fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 15:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Error-State Kalman Filter (#22262)

Browse Source 
* ekf derivation: change to error state formulation
* ekf2: update auto-generated code for error-state
* ekf2: adjust ekf2 code for error state formulation
* ekf2_tests: adjust unit tests for error-state EKF
* update change indicator for error-state EKF
* ekf2_derivation: allow disabling mag and wind states

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
This commit is contained in:
Mathieu Bresciani
2023-10-31 10:02:18 -04:00
committed by GitHub
co-authored by bresch
parent d7f388e590
commit 0d6c2c8ce9
51 changed files with 2667 additions and 4089 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_helper/comparison_helper.cpp
+3 -61
View File
@@ -38,12 +38,12 @@ float randf()
return (float)rand() / (float)RAND_MAX;
}
SquareMatrix24f createRandomCovarianceMatrix24f()
SquareMatrixState createRandomCovarianceMatrix()
{
// Create a symmetric square matrix
SquareMatrix24f P;
SquareMatrixState P;
for (int col = 0; col <= 23; col++) {
for (int col = 0; col < State::size; col++) {
for (int row = 0; row <= col; row++) {
if (row == col) {
P(row, col) = randf();
@@ -59,61 +59,3 @@ SquareMatrix24f createRandomCovarianceMatrix24f()
return P;
}
DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2)
{
DiffRatioReport report = {};
for (int row = 0; row < 24; row++) {
float diff_fraction;
if (fabsf(v1(row)) > FLT_EPSILON) {
diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v1(row));
} else if (fabsf(v2(row)) > FLT_EPSILON) {
diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v2(row));
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > report.max_diff_fraction) {
report.max_diff_fraction = diff_fraction;
report.max_row = row;
report.max_v1 = v1(row);
report.max_v2 = v2(row);
}
}
return report;
}
DiffRatioReport computeDiffRatioSquareMatrix24f(const SquareMatrix24f &m1, const SquareMatrix24f &m2)
{
DiffRatioReport report = {};
for (int row = 0; row < 24; row++) {
for (int col = 0; col < 24; col++) {
float diff_fraction;
if (fabsf(m1(row, col)) > FLT_EPSILON) {
diff_fraction = fabsf(m2(row, col) - m1(row, col)) / fabsf(m1(row, col));
} else if (fabsf(m2(row, col)) > FLT_EPSILON) {
diff_fraction = fabsf(m2(row, col) - m1(row, col)) / fabsf(m2(row, col));
} else {
diff_fraction = 0.0f;
}
if (diff_fraction > report.max_diff_fraction) {
report.max_diff_fraction = diff_fraction;
report.max_row = row;
report.max_v1 = m1(row, col);
report.max_v2 = m2(row, col);
}
}
}
return report;
}
src/modules/ekf2/test/test_helper/comparison_helper.h
+3 -16
View File
@@ -36,24 +36,11 @@
#include "EKF/ekf.h"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
struct DiffRatioReport {
float max_diff_fraction;
float max_row;
float max_v1;
float max_v2;
};
typedef matrix::Vector<float, State::size> VectorState;
typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
float randf();
// Create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f createRandomCovarianceMatrix24f();
// Find largest element-wise difference as a fraction of v1 or v2
DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2);
DiffRatioReport computeDiffRatioSquareMatrix24f(const SquareMatrix24f &m1, const SquareMatrix24f &m2);
SquareMatrixState createRandomCovarianceMatrix();
#endif