fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 17:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib delete Matrix, Quaternion, Vector

Browse Source
This commit is contained in:
Daniel Agar
2018-03-28 17:05:42 -04:00
parent 0d7b5c4a4e
commit 222a91c6be
32 changed files with 160 additions and 1746 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/calibration_routines.cpp
+17 -23
View File
@@ -51,6 +51,7 @@
#include <lib/ecl/geo/geo.h>
#include <string.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/sensor_combined.h>
@@ -270,13 +271,10 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
float fitness = _fitness;
float fit1 = 0.0f, fit2 = 0.0f;
float JTJ[16];
float JTJ2[16];
float JTFI[4];
matrix::SquareMatrix<float, 4> JTJ;
matrix::SquareMatrix<float, 4> JTJ2;
float JTFI[4] = {};
float residual = 0.0f;
memset(JTJ, 0, sizeof(JTJ));
memset(JTJ2, 0, sizeof(JTJ2));
memset(JTFI, 0, sizeof(JTFI));
// Gauss Newton Part common for all kind of extensions including LM
for (uint16_t k = 0; k < _samples_collected; k++) {
@@ -299,8 +297,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
for (uint8_t i = 0; i < 4; i++) {
// compute JTJ
for (uint8_t j = 0; j < 4; j++) {
JTJ[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j];
JTJ2[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
JTJ2(i, j) += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
}
JTFI[i] += sphere_jacob[i] * residual;
@@ -315,22 +313,22 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
for (uint8_t i = 0; i < 4; i++) {
JTJ[i * 4 + i] += _sphere_lambda;
JTJ2[i * 4 + i] += _sphere_lambda / lma_damping;
JTJ(i, i) += _sphere_lambda;
JTJ2(i, i) += _sphere_lambda / lma_damping;
}
if (!inverse4x4(JTJ, JTJ)) {
if (!JTJ.I(JTJ)) {
return -1;
}
if (!inverse4x4(JTJ2, JTJ2)) {
if (!JTJ2.I(JTJ2)) {
return -1;
}
for (uint8_t row = 0; row < 4; row++) {
for (uint8_t col = 0; col < 4; col++) {
fit1_params[row] -= JTFI[col] * JTJ[row * 4 + col];
fit2_params[row] -= JTFI[col] * JTJ2[row * 4 + col];
fit1_params[row] -= JTFI[col] * JTJ(row, col);
fit2_params[row] -= JTFI[col] * JTJ2(row, col);
}
}
@@ -395,15 +393,13 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
const float lma_damping = 10.0f;
float _samples_collected = size;
float fitness = _fitness;
float fit1 = 0.0f, fit2 = 0.0f;
float fit1 = 0.0f;
float fit2 = 0.0f;
float JTJ[81];
float JTJ2[81];
float JTFI[9];
float JTJ[81] = {};
float JTJ2[81] = {};
float JTFI[9] = {};
float residual = 0.0f;
memset(JTJ, 0, sizeof(JTJ));
memset(JTJ2, 0, sizeof(JTJ2));
memset(JTFI, 0, sizeof(JTFI));
float ellipsoid_jacob[9];
// Gauss Newton Part common for all kind of extensions including LM
@@ -461,8 +457,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
return -1;
}
for (uint8_t row = 0; row < 9; row++) {
for (uint8_t col = 0; col < 9; col++) {
fit1_params[row] -= JTFI[col] * JTJ[row * 9 + col];