PX4 Firmware
PX4 Autopilot Software http://px4.io
SquareMatrix.hpp
Go to the documentation of this file.
1 /**
2  * @file SquareMatrix.hpp
3  *
4  * A square matrix
5  *
6  * @author James Goppert <james.goppert@gmail.com>
7  */
8 
9 #pragma once
10 
11 #include "math.hpp"
12 
13 namespace matrix
14 {
15 
16 template <typename Type, size_t M, size_t N>
17 class Matrix;
18 
19 template <typename Type, size_t M>
20 class Vector;
21 
22 template <typename Type, size_t P, size_t Q, size_t M, size_t N>
23 class Slice;
24 
25 template<typename Type, size_t M>
26 class SquareMatrix : public Matrix<Type, M, M>
27 {
28 public:
29  SquareMatrix() = default;
30 
31  explicit SquareMatrix(const Type data_[M][M]) :
32  Matrix<Type, M, M>(data_)
33  {
34  }
35 
36  explicit SquareMatrix(const Type data_[M*M]) :
37  Matrix<Type, M, M>(data_)
38  {
39  }
40 
42  Matrix<Type, M, M>(other)
43  {
44  }
45 
46  template<size_t P, size_t Q>
47  SquareMatrix(const Slice<Type, M, M, P, Q>& in_slice) : Matrix<Type, M, M>(in_slice)
48  {
49  }
50 
52  {
54  return *this;
55  }
56 
57  template <size_t P, size_t Q>
59  {
61  return *this;
62  }
63 
64  // inverse alias
65  inline SquareMatrix<Type, M> I() const
66  {
68  if (inv(*this, i)) {
69  return i;
70  } else {
71  i.setZero();
72  return i;
73  }
74  }
75 
76  // inverse alias
77  inline bool I(SquareMatrix<Type, M> &i) const
78  {
79  return inv(*this, i);
80  }
81 
82 
84  {
85  Vector<Type, M> res;
86  const SquareMatrix<Type, M> &self = *this;
87 
88  for (size_t i = 0; i < M; i++) {
89  res(i) = self(i, i);
90  }
91  return res;
92  }
93 
94  // get matrix upper right triangle in a row-major vector format
95  Vector<Type, M * (M + 1) / 2> upper_right_triangle() const
96  {
97  Vector<Type, M * (M + 1) / 2> res;
98  const SquareMatrix<Type, M> &self = *this;
99 
100  unsigned idx = 0;
101  for (size_t x = 0; x < M; x++) {
102  for (size_t y = x; y < M; y++) {
103  res(idx) = self(x, y);
104  ++idx;
105  }
106  }
107 
108  return res;
109  }
110 
111  Type trace() const
112  {
113  Type res = 0;
114  const SquareMatrix<Type, M> &self = *this;
115 
116  for (size_t i = 0; i < M; i++) {
117  res += self(i, i);
118  }
119  return res;
120  }
121 };
122 
124 
125 template<typename Type, size_t M>
128  m.setIdentity();
129  return m;
130 }
131 
132 template<typename Type, size_t M>
135  for (size_t i=0; i<M; i++) {
136  m(i,i) = d(i);
137  }
138  return m;
139 }
140 
141 template<typename Type, size_t M>
143 {
145  SquareMatrix<Type, M> A_pow = A;
146  res.setIdentity();
147  size_t i_factorial = 1;
148  for (size_t i=1; i<=order; i++) {
149  i_factorial *= i;
150  res += A_pow / Type(i_factorial);
151  A_pow *= A_pow;
152  }
153 
154  return res;
155 }
156 
157 
158 /**
159  * inverse based on LU factorization with partial pivotting
160  */
161 template<typename Type, size_t M>
163 {
165  L.setIdentity();
166  SquareMatrix<Type, M> U = A;
168  P.setIdentity();
169 
170  //printf("A:\n"); A.print();
171 
172  // for all diagonal elements
173  for (size_t n = 0; n < M; n++) {
174 
175  // if diagonal is zero, swap with row below
176  if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
177  //printf("trying pivot for row %d\n",n);
178  for (size_t i = n + 1; i < M; i++) {
179 
180  //printf("\ttrying row %d\n",i);
181  if (fabs(static_cast<float>(U(i, n))) > 1e-8f) {
182  //printf("swapped %d\n",i);
183  U.swapRows(i, n);
184  P.swapRows(i, n);
185  L.swapRows(i, n);
186  L.swapCols(i, n);
187  break;
188  }
189  }
190  }
191 
192 #ifdef MATRIX_ASSERT
193  //printf("A:\n"); A.print();
194  //printf("U:\n"); U.print();
195  //printf("P:\n"); P.print();
196  //fflush(stdout);
197  //ASSERT(fabs(U(n, n)) > 1e-8f);
198 #endif
199 
200  // failsafe, return zero matrix
201  if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
202  return false;
203  }
204 
205  // for all rows below diagonal
206  for (size_t i = (n + 1); i < M; i++) {
207  L(i, n) = U(i, n) / U(n, n);
208 
209  // add i-th row and n-th row
210  // multiplied by: -a(i,n)/a(n,n)
211  for (size_t k = n; k < M; k++) {
212  U(i, k) -= L(i, n) * U(n, k);
213  }
214  }
215  }
216 
217  //printf("L:\n"); L.print();
218  //printf("U:\n"); U.print();
219 
220  // solve LY=P*I for Y by forward subst
221  //SquareMatrix<Type, M> Y = P;
222 
223  // for all columns of Y
224  for (size_t c = 0; c < M; c++) {
225  // for all rows of L
226  for (size_t i = 0; i < M; i++) {
227  // for all columns of L
228  for (size_t j = 0; j < i; j++) {
229  // for all existing y
230  // subtract the component they
231  // contribute to the solution
232  P(i, c) -= L(i, j) * P(j, c);
233  }
234 
235  // divide by the factor
236  // on current
237  // term to be solved
238  // Y(i,c) /= L(i,i);
239  // but L(i,i) = 1.0
240  }
241  }
242 
243  //printf("Y:\n"); Y.print();
244 
245  // solve Ux=y for x by back subst
246  //SquareMatrix<Type, M> X = Y;
247 
248  // for all columns of X
249  for (size_t c = 0; c < M; c++) {
250  // for all rows of U
251  for (size_t k = 0; k < M; k++) {
252  // have to go in reverse order
253  size_t i = M - 1 - k;
254 
255  // for all columns of U
256  for (size_t j = i + 1; j < M; j++) {
257  // for all existing x
258  // subtract the component they
259  // contribute to the solution
260  P(i, c) -= U(i, j) * P(j, c);
261  }
262 
263  // divide by the factor
264  // on current
265  // term to be solved
266  //
267  // we know that U(i, i) != 0 from above
268  P(i, c) /= U(i, i);
269  }
270  }
271 
272  //check sanity of results
273  for (size_t i = 0; i < M; i++) {
274  for (size_t j = 0; j < M; j++) {
275  if (!is_finite(P(i,j))) {
276  return false;
277  }
278  }
279  }
280  //printf("X:\n"); X.print();
281  inv = P;
282  return true;
283 }
284 
285 /**
286  * inverse based on LU factorization with partial pivotting
287  */
288 template<typename Type, size_t M>
290 {
292  if (inv(A, i)) {
293  return i;
294  } else {
295  i.setZero();
296  return i;
297  }
298 }
299 
300 /**
301  * cholesky decomposition
302  *
303  * Note: A must be positive definite
304  */
305 template<typename Type, size_t M>
307 {
309  for (size_t j = 0; j < M; j++) {
310  for (size_t i = j; i < M; i++) {
311  if (i==j) {
312  float sum = 0;
313  for (size_t k = 0; k < j; k++) {
314  sum += L(j, k)*L(j, k);
315  }
316  Type res = A(j, j) - sum;
317  if (res <= 0) {
318  L(j, j) = 0;
319  } else {
320  L(j, j) = sqrt(res);
321  }
322  } else {
323  float sum = 0;
324  for (size_t k = 0; k < j; k++) {
325  sum += L(i, k)*L(j, k);
326  }
327  if (L(j, j) <= 0) {
328  L(i, j) = 0;
329  } else {
330  L(i, j) = (A(i, j) - sum)/L(j, j);
331  }
332  }
333  }
334  }
335  return L;
336 }
337 
338 /**
339  * cholesky inverse
340  *
341  * TODO: Check if gaussian elimination jumps straight to back-substitution
342  * for L or we need to do it manually. Will impact speed otherwise.
343  */
344 template<typename Type, size_t M>
346 {
347  SquareMatrix<Type, M> L_inv = inv(cholesky(A));
348  return L_inv.T()*L_inv;
349 }
350 
352 
353 } // namespace matrix
354 
355 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Matrix< Type, N, M > T() const
Definition: Matrix.hpp:368
SquareMatrix< float, 3 > SquareMatrix3f
SquareMatrix(const Matrix< Type, M, M > &other)
Matrix< Type, M, N > & operator=(const Matrix< Type, M, N > &other)
Definition: Matrix.hpp:107
SquareMatrix< float, 3 > Matrix3f
bool I(SquareMatrix< Type, M > &i) const
SquareMatrix< Type, M > eye()
SquareMatrix< Type, M > cholesky(const SquareMatrix< Type, M > &A)
cholesky decomposition
SquareMatrix(const Type data_[M][M])
#define FLT_EPSILON
void swapRows(size_t a, size_t b)
Definition: Matrix.hpp:462
SquareMatrix(const Slice< Type, M, M, P, Q > &in_slice)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
SquareMatrix< Type, M > I() const
bool is_finite(Type x)
Vector< Type, M > diag() const
SquareMatrix(const Type data_[M *M])
SquareMatrix< Type, M > & operator=(const Matrix< Type, M, M > &other)
SquareMatrix< Type, M > expm(const Matrix< Type, M, M > &A, size_t order=5)
P[0][0]
Definition: quatCovMat.c:44
SquareMatrix< Type, M > choleskyInv(const SquareMatrix< Type, M > &A)
cholesky inverse
Vector< Type, M *(M+1)/2 > upper_right_triangle() const
SquareMatrix< Type, M > & operator=(const Slice< Type, M, M, P, Q > &in_slice)
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Definition: Dual.hpp:188
void swapCols(size_t a, size_t b)
Definition: Matrix.hpp:477