PX4 Firmware
PX4 Autopilot Software http://px4.io
LeastSquaresSolver.hpp
Go to the documentation of this file.
1 /**
2  * @file LeastSquaresSolver.hpp
3  *
4  * Least Squares Solver using QR householder decomposition.
5  * It calculates x for Ax = b.
6  * A = Q*R
7  * where R is an upper triangular matrix.
8  *
9  * R*x = Q^T*b
10  * This is efficiently solved for x because of the upper triangular property of R.
11  *
12  * @author Bart Slinger <bartslinger@gmail.com>
13  */
14 
15 #pragma once
16 
17 #include "math.hpp"
18 
19 namespace matrix {
20 
21 template<typename Type, size_t M, size_t N>
23 {
24 public:
25 
26  /**
27  * @brief Class calculates QR decomposition which can be used for linear
28  * least squares
29  * @param A Matrix of size MxN
30  *
31  * Initialize the class with a MxN matrix. The constructor starts the
32  * QR decomposition. This class does not check the rank of the matrix.
33  * The user needs to make sure that rank(A) = N and M >= N.
34  */
36  {
37  static_assert(M >= N, "Matrix dimension should be M >= N");
38 
39  // Copy contentents of matrix A
40  _A = A;
41 
42  for (size_t j = 0; j < N; j++) {
43  Type normx = Type(0);
44  for (size_t i = j; i < M; i++) {
45  normx += _A(i,j) * _A(i,j);
46  }
47  normx = sqrt(normx);
48  Type s = _A(j,j) > 0 ? Type(-1) : Type(1);
49  Type u1 = _A(j,j) - s*normx;
50  // prevent divide by zero
51  // also covers u1. normx is never negative
52  if (normx < Type(1e-8)) {
53  break;
54  }
55  Type w[M] = {};
56  w[0] = Type(1);
57  for (size_t i = j+1; i < M; i++) {
58  w[i-j] = _A(i,j) / u1;
59  _A(i,j) = w[i-j];
60  }
61  _A(j,j) = s*normx;
62  _tau(j) = -s*u1/normx;
63 
64  for (size_t k = j+1; k < N; k++) {
65  Type tmp = Type(0);
66  for (size_t i = j; i < M; i++) {
67  tmp += w[i-j] * _A(i,k);
68  }
69  for (size_t i = j; i < M; i++) {
70  _A(i,k) -= _tau(j) * w[i-j] * tmp;
71  }
72  }
73 
74  }
75  }
76 
77  /**
78  * @brief qtb Calculate Q^T * b
79  * @param b
80  * @return Q^T*b
81  *
82  * This function calculates Q^T * b. This is useful for the solver
83  * because R*x = Q^T*b.
84  */
86  Vector<Type, M> qtbv = b;
87 
88  for (size_t j = 0; j < N; j++) {
89  Type w[M];
90  w[0] = Type(1);
91  // fill vector w
92  for (size_t i = j+1; i < M; i++) {
93  w[i-j] = _A(i,j);
94  }
95  Type tmp = Type(0);
96  for (size_t i = j; i < M; i++) {
97  tmp += w[i-j] * qtbv(i);
98  }
99 
100  for (size_t i = j; i < M; i++) {
101  qtbv(i) -= _tau(j) * w[i-j] * tmp;
102  }
103  }
104  return qtbv;
105  }
106 
107  /**
108  * @brief Solve Ax=b for x
109  * @param b
110  * @return Vector x
111  *
112  * Find x in the equation Ax = b.
113  * A is provided in the initializer of the class.
114  */
116  Vector<Type, M> qtbv = qtb(b);
117  Vector<Type, N> x;
118 
119  // size_t is unsigned and wraps i = 0 - 1 to i > N
120  for (size_t i = N - 1; i < N; i--) {
121  printf("i %d\n", static_cast<int>(i));
122  x(i) = qtbv(i);
123  for (size_t r = i+1; r < N; r++) {
124  x(i) -= _A(i,r) * x(r);
125  }
126  // divide by zero, return vector of zeros
127  if (isEqualF(_A(i,i), Type(0), Type(1e-8))) {
128  for (size_t z = 0; z < N; z++) {
129  x(z) = Type(0);
130  }
131  break;
132  }
133  x(i) /= _A(i,i);
134  }
135  return x;
136  }
137 
138 private:
141 
142 };
143 
144 } // namespace matrix
145 
146 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
LeastSquaresSolver(const Matrix< Type, M, N > &A)
Class calculates QR decomposition which can be used for linear least squares.
Vector< Type, N > solve(const Vector< Type, M > &b)
Solve Ax=b for x.
Vector< Type, M > qtb(const Vector< Type, M > &b)
qtb Calculate Q^T * b
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Definition: Dual.hpp:188