fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 08:23:34 +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/EKF/covariance.cpp
+2 -5
View File
@@ -43,7 +43,6 @@
#include "ekf.h"
#include <ekf_derivation/generated/predict_covariance.h>
#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
#include <math.h>
#include <mathlib/mathlib.h>
@@ -368,10 +367,8 @@ void Ekf::resetQuatCov(const float yaw_noise)
void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
{
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov);
q_cov.copyLowerToUpperTriangle();
resetStateCovariance<State::quat_nominal>(q_cov);
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov_ned = diag(rot_var_ned);
resetStateCovariance<State::quat_nominal>(_R_to_earth.T() * q_cov_ned * _R_to_earth);
}
#if defined(CONFIG_EKF2_MAGNETOMETER)