fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:43:35 +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_EKF_initialization.cpp
+7 -7
View File
@@ -78,10 +78,10 @@ public:
void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f)
{
const matrix::Vector4f quat_variance = _ekf->getQuaternionVariance();
const matrix::Vector3f quat_variance = _ekf->getQuaternionVariance();
EXPECT_TRUE(quat_variance(0) > quat_variance_limit) << "quat_variance(3): " << quat_variance(0);
EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1);
EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2);
EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3);
}
void yawVarianceBigEnoughAfterHeadingReset()
@@ -209,12 +209,12 @@ TEST_F(EkfInitializationTest, gyroBias)
if (fabsf(accel_bias(2)) > 0.3f) {
// Print state covariance and correlation matrices for debugging
const matrix::SquareMatrix<float, 24> P = _ekf->covariances();
const auto P = _ekf->covariances();
printf("State covariance:\n");
for (int i = 0; i <= 15; i++) {
for (int j = 0; j <= 15; j++) {
for (int i = 0; i <= State::size; i++) {
for (int j = 0; j <= State::size; j++) {
printf("%.3fe-9 ", ((double)P(i, j)) * 1e9);
}
@@ -224,10 +224,10 @@ TEST_F(EkfInitializationTest, gyroBias)
printf("State correlation:\n");
printf("\t0\t1\t2\t3\t4\t5\t6\t7\t8\t9\t10\t11\t12\t13\t14\t15\n");
for (uint8_t i = 0; i <= 15; i++) {
for (uint8_t i = 0; i <= State::size; i++) {
printf("%d| ", i);
for (uint8_t j = 0; j <= 15; j++) {
for (uint8_t j = 0; j <= State::size; j++) {
float corr = sqrtf(fabsf(P(i, i) * P(j, j)));
if (corr > 0.0f) {