42 #include <px4_platform_common/defines.h> 53 float *
mat_mul(
float *A,
float *B, uint8_t n)
55 float *ret =
new float[n * n];
56 memset(ret, 0.0
f, n * n *
sizeof(
float));
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];
69 static inline void swap(
float &a,
float &b)
86 static void mat_pivot(
float *A,
float *pivot, uint8_t n)
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);
94 for (uint8_t i = 0; i < n; i++) {
97 for (uint8_t j = i; j < n; j++) {
98 if (fabsf(A[j * n + i]) > fabsf(A[max_j * n + i])) {
104 for (uint8_t k = 0; k < n; k++) {
105 swap(pivot[i * n + k], pivot[max_j * n + k]);
122 for (
int i = 0; i < n; i++) {
123 out[i * n + i] = 1 / L[i * n + i];
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];
130 out[j * n + i] /= L[j * n + j];
146 for (
int i = n - 1; i >= 0; i--) {
147 out[i * n + i] = 1 / U[i * n + i];
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];
154 out[j * n + i] /= U[j * n + j];
170 memset(L, 0, n * n *
sizeof(
float));
171 memset(U, 0, n * n *
sizeof(
float));
172 memset(P, 0, n * n *
sizeof(
float));
175 float *APrime =
mat_mul(P, A, n);
177 for (uint8_t i = 0; i < n; i++) {
181 for (uint8_t i = 0; i < n; i++) {
182 for (uint8_t j = 0; j < n; j++) {
184 U[j * n + i] = APrime[j * n + i];
186 for (uint8_t k = 0; k < j; k++) {
187 U[j * n + i] -= L[j * n + k] * U[k * n + i];
192 L[j * n + i] = APrime[j * n + i];
194 for (uint8_t k = 0; k < i; k++) {
195 L[j * n + i] -= L[j * n + k] * U[k * n + i];
198 L[j * n + i] /= U[i * n + i];
219 L =
new float[n * n];
220 U =
new float[n * n];
221 P =
new float[n * n];
224 float *L_inv =
new float[n * n];
225 float *U_inv =
new float[n * n];
227 memset(L_inv, 0, n * n *
sizeof(
float));
230 memset(U_inv, 0, n * n *
sizeof(
float));
237 float *inv_unpivoted =
mat_mul(U_inv, L_inv, n);
238 float *inv_pivoted =
mat_mul(inv_unpivoted, P, n);
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])) {
249 memcpy(inv, inv_pivoted, n * n *
sizeof(
float));
252 delete[] inv_pivoted;
253 delete[] inv_unpivoted;
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];
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];
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] -
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] +
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];
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];
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] +
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] -
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] -
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] +
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] -
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] +
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] -
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] +
363 inv[11] = -m[0] * m[5] * m[11] +
365 m[4] * m[1] * m[11] -
370 inv[15] = m[0] * m[5] * m[10] -
372 m[4] * m[1] * m[10] +
377 det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
379 if (fabsf(det) < 1.1755e-38
f) {
385 for (i = 0; i < 16; i++) {
386 invOut[i] = inv[i] * det;
float * mat_mul(float *A, float *B, uint8_t n)
static void swap(float &a, float &b)
Matrix algebra on raw arrays.
static void mat_pivot(float *A, float *pivot, uint8_t n)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
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)
bool mat_inverse(float *A, float *inv, uint8_t n)
bool inverse4x4(float m[], float invOut[])
static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n)
static void mat_forward_sub(float *L, float *out, uint8_t n)