PX4 Firmware
PX4 Autopilot Software http://px4.io
matrix_alg.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2013 PX4 Development Team. All rights reserved.
4  * Author: Siddharth Bharat Purohit <sidbpurohit@gmail.com>
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file matrix_alg.cpp
37  *
38  * Matrix algebra on raw arrays
39  */
40 
41 #include "matrix_alg.h"
42 #include <px4_platform_common/defines.h>
43 
44 /*
45  * Does matrix multiplication of two regular/square matrices
46  *
47  * @param A, Matrix A
48  * @param B, Matrix B
49  * @param n, dimemsion of square matrices
50  * @returns multiplied matrix i.e. A*B
51  */
52 
53 float *mat_mul(float *A, float *B, uint8_t n)
54 {
55  float *ret = new float[n * n];
56  memset(ret, 0.0f, n * n * sizeof(float));
57 
58  for (uint8_t i = 0; i < n; i++) {
59  for (uint8_t j = 0; j < n; j++) {
60  for (uint8_t k = 0; k < n; k++) {
61  ret[i * n + j] += A[i * n + k] * B[k * n + j];
62  }
63  }
64  }
65 
66  return ret;
67 }
68 
69 static inline void swap(float &a, float &b)
70 {
71  float c;
72  c = a;
73  a = b;
74  b = c;
75 }
76 
77 /*
78  * calculates pivot matrix such that all the larger elements in the row are on diagonal
79  *
80  * @param A, input matrix matrix
81  * @param pivot
82  * @param n, dimenstion of square matrix
83  * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful
84  */
85 
86 static void mat_pivot(float *A, float *pivot, uint8_t n)
87 {
88  for (uint8_t i = 0; i < n; i++) {
89  for (uint8_t j = 0; j < n; j++) {
90  pivot[i * n + j] = (i == j);
91  }
92  }
93 
94  for (uint8_t i = 0; i < n; i++) {
95  uint8_t max_j = i;
96 
97  for (uint8_t j = i; j < n; j++) {
98  if (fabsf(A[j * n + i]) > fabsf(A[max_j * n + i])) {
99  max_j = j;
100  }
101  }
102 
103  if (max_j != i) {
104  for (uint8_t k = 0; k < n; k++) {
105  swap(pivot[i * n + k], pivot[max_j * n + k]);
106  }
107  }
108  }
109 }
110 
111 /*
112  * calculates matrix inverse of Lower trangular matrix using forward substitution
113  *
114  * @param L, lower triangular matrix
115  * @param out, Output inverted lower triangular matrix
116  * @param n, dimension of matrix
117  */
118 
119 static void mat_forward_sub(float *L, float *out, uint8_t n)
120 {
121  // Forward substitution solve LY = I
122  for (int i = 0; i < n; i++) {
123  out[i * n + i] = 1 / L[i * n + i];
124 
125  for (int j = i + 1; j < n; j++) {
126  for (int k = i; k < j; k++) {
127  out[j * n + i] -= L[j * n + k] * out[k * n + i];
128  }
129 
130  out[j * n + i] /= L[j * n + j];
131  }
132  }
133 }
134 
135 /*
136  * calculates matrix inverse of Upper trangular matrix using backward substitution
137  *
138  * @param U, upper triangular matrix
139  * @param out, Output inverted upper triangular matrix
140  * @param n, dimension of matrix
141  */
142 
143 static void mat_back_sub(float *U, float *out, uint8_t n)
144 {
145  // Backward Substitution solve UY = I
146  for (int i = n - 1; i >= 0; i--) {
147  out[i * n + i] = 1 / U[i * n + i];
148 
149  for (int j = i - 1; j >= 0; j--) {
150  for (int k = i; k > j; k--) {
151  out[j * n + i] -= U[j * n + k] * out[k * n + i];
152  }
153 
154  out[j * n + i] /= U[j * n + j];
155  }
156  }
157 }
158 
159 /*
160  * Decomposes square matrix into Lower and Upper triangular matrices such that
161  * A*P = L*U, where P is the pivot matrix
162  * ref: http://rosettacode.org/wiki/LU_decomposition
163  * @param U, upper triangular matrix
164  * @param out, Output inverted upper triangular matrix
165  * @param n, dimension of matrix
166  */
167 
168 static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n)
169 {
170  memset(L, 0, n * n * sizeof(float));
171  memset(U, 0, n * n * sizeof(float));
172  memset(P, 0, n * n * sizeof(float));
173  mat_pivot(A, P, n);
174 
175  float *APrime = mat_mul(P, A, n);
176 
177  for (uint8_t i = 0; i < n; i++) {
178  L[i * n + i] = 1;
179  }
180 
181  for (uint8_t i = 0; i < n; i++) {
182  for (uint8_t j = 0; j < n; j++) {
183  if (j <= i) {
184  U[j * n + i] = APrime[j * n + i];
185 
186  for (uint8_t k = 0; k < j; k++) {
187  U[j * n + i] -= L[j * n + k] * U[k * n + i];
188  }
189  }
190 
191  if (j >= i) {
192  L[j * n + i] = APrime[j * n + i];
193 
194  for (uint8_t k = 0; k < i; k++) {
195  L[j * n + i] -= L[j * n + k] * U[k * n + i];
196  }
197 
198  L[j * n + i] /= U[i * n + i];
199  }
200  }
201  }
202 
203  delete[] APrime;
204 }
205 
206 /*
207  * matrix inverse code for any square matrix using LU decomposition
208  * inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
209  * ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
210  * @param m, input 4x4 matrix
211  * @param inv, Output inverted 4x4 matrix
212  * @param n, dimension of square matrix
213  * @returns false = matrix is Singular, true = matrix inversion successful
214  */
215 bool mat_inverse(float *A, float *inv, uint8_t n)
216 {
217  float *L, *U, *P;
218  bool ret = true;
219  L = new float[n * n];
220  U = new float[n * n];
221  P = new float[n * n];
222  mat_LU_decompose(A, L, U, P, n);
223 
224  float *L_inv = new float[n * n];
225  float *U_inv = new float[n * n];
226 
227  memset(L_inv, 0, n * n * sizeof(float));
228  mat_forward_sub(L, L_inv, n);
229 
230  memset(U_inv, 0, n * n * sizeof(float));
231  mat_back_sub(U, U_inv, n);
232 
233  // decomposed matrices no longer required
234  delete[] L;
235  delete[] U;
236 
237  float *inv_unpivoted = mat_mul(U_inv, L_inv, n);
238  float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
239 
240  //check sanity of results
241  for (uint8_t i = 0; i < n; i++) {
242  for (uint8_t j = 0; j < n; j++) {
243  if (!PX4_ISFINITE(inv_pivoted[i * n + j])) {
244  ret = false;
245  }
246  }
247  }
248 
249  memcpy(inv, inv_pivoted, n * n * sizeof(float));
250 
251  //free memory
252  delete[] inv_pivoted;
253  delete[] inv_unpivoted;
254  delete[] P;
255  delete[] U_inv;
256  delete[] L_inv;
257  return ret;
258 }
259 
260 bool inverse4x4(float m[], float invOut[])
261 {
262  float inv[16], det;
263  uint8_t i;
264 
265  inv[0] = m[5] * m[10] * m[15] -
266  m[5] * m[11] * m[14] -
267  m[9] * m[6] * m[15] +
268  m[9] * m[7] * m[14] +
269  m[13] * m[6] * m[11] -
270  m[13] * m[7] * m[10];
271 
272  inv[4] = -m[4] * m[10] * m[15] +
273  m[4] * m[11] * m[14] +
274  m[8] * m[6] * m[15] -
275  m[8] * m[7] * m[14] -
276  m[12] * m[6] * m[11] +
277  m[12] * m[7] * m[10];
278 
279  inv[8] = m[4] * m[9] * m[15] -
280  m[4] * m[11] * m[13] -
281  m[8] * m[5] * m[15] +
282  m[8] * m[7] * m[13] +
283  m[12] * m[5] * m[11] -
284  m[12] * m[7] * m[9];
285 
286  inv[12] = -m[4] * m[9] * m[14] +
287  m[4] * m[10] * m[13] +
288  m[8] * m[5] * m[14] -
289  m[8] * m[6] * m[13] -
290  m[12] * m[5] * m[10] +
291  m[12] * m[6] * m[9];
292 
293  inv[1] = -m[1] * m[10] * m[15] +
294  m[1] * m[11] * m[14] +
295  m[9] * m[2] * m[15] -
296  m[9] * m[3] * m[14] -
297  m[13] * m[2] * m[11] +
298  m[13] * m[3] * m[10];
299 
300  inv[5] = m[0] * m[10] * m[15] -
301  m[0] * m[11] * m[14] -
302  m[8] * m[2] * m[15] +
303  m[8] * m[3] * m[14] +
304  m[12] * m[2] * m[11] -
305  m[12] * m[3] * m[10];
306 
307  inv[9] = -m[0] * m[9] * m[15] +
308  m[0] * m[11] * m[13] +
309  m[8] * m[1] * m[15] -
310  m[8] * m[3] * m[13] -
311  m[12] * m[1] * m[11] +
312  m[12] * m[3] * m[9];
313 
314  inv[13] = m[0] * m[9] * m[14] -
315  m[0] * m[10] * m[13] -
316  m[8] * m[1] * m[14] +
317  m[8] * m[2] * m[13] +
318  m[12] * m[1] * m[10] -
319  m[12] * m[2] * m[9];
320 
321  inv[2] = m[1] * m[6] * m[15] -
322  m[1] * m[7] * m[14] -
323  m[5] * m[2] * m[15] +
324  m[5] * m[3] * m[14] +
325  m[13] * m[2] * m[7] -
326  m[13] * m[3] * m[6];
327 
328  inv[6] = -m[0] * m[6] * m[15] +
329  m[0] * m[7] * m[14] +
330  m[4] * m[2] * m[15] -
331  m[4] * m[3] * m[14] -
332  m[12] * m[2] * m[7] +
333  m[12] * m[3] * m[6];
334 
335  inv[10] = m[0] * m[5] * m[15] -
336  m[0] * m[7] * m[13] -
337  m[4] * m[1] * m[15] +
338  m[4] * m[3] * m[13] +
339  m[12] * m[1] * m[7] -
340  m[12] * m[3] * m[5];
341 
342  inv[14] = -m[0] * m[5] * m[14] +
343  m[0] * m[6] * m[13] +
344  m[4] * m[1] * m[14] -
345  m[4] * m[2] * m[13] -
346  m[12] * m[1] * m[6] +
347  m[12] * m[2] * m[5];
348 
349  inv[3] = -m[1] * m[6] * m[11] +
350  m[1] * m[7] * m[10] +
351  m[5] * m[2] * m[11] -
352  m[5] * m[3] * m[10] -
353  m[9] * m[2] * m[7] +
354  m[9] * m[3] * m[6];
355 
356  inv[7] = m[0] * m[6] * m[11] -
357  m[0] * m[7] * m[10] -
358  m[4] * m[2] * m[11] +
359  m[4] * m[3] * m[10] +
360  m[8] * m[2] * m[7] -
361  m[8] * m[3] * m[6];
362 
363  inv[11] = -m[0] * m[5] * m[11] +
364  m[0] * m[7] * m[9] +
365  m[4] * m[1] * m[11] -
366  m[4] * m[3] * m[9] -
367  m[8] * m[1] * m[7] +
368  m[8] * m[3] * m[5];
369 
370  inv[15] = m[0] * m[5] * m[10] -
371  m[0] * m[6] * m[9] -
372  m[4] * m[1] * m[10] +
373  m[4] * m[2] * m[9] +
374  m[8] * m[1] * m[6] -
375  m[8] * m[2] * m[5];
376 
377  det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
378 
379  if (fabsf(det) < 1.1755e-38f) {
380  return false;
381  }
382 
383  det = 1.0f / det;
384 
385  for (i = 0; i < 16; i++) {
386  invOut[i] = inv[i] * det;
387  }
388 
389  return true;
390 }
float * mat_mul(float *A, float *B, uint8_t n)
Definition: matrix_alg.cpp:53
static void swap(float &a, float &b)
Definition: matrix_alg.cpp:69
Matrix algebra on raw arrays.
static void mat_pivot(float *A, float *pivot, uint8_t n)
Definition: matrix_alg.cpp:86
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
static void mat_back_sub(float *U, float *out, uint8_t n)
Definition: matrix_alg.cpp:143
bool mat_inverse(float *A, float *inv, uint8_t n)
Definition: matrix_alg.cpp:215
bool inverse4x4(float m[], float invOut[])
Definition: matrix_alg.cpp:260
static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n)
Definition: matrix_alg.cpp:168
static void mat_forward_sub(float *L, float *out, uint8_t n)
Definition: matrix_alg.cpp:119
P[0][0]
Definition: quatCovMat.c:44