EKF2: Error-State Kalman Filter (#22262)

* 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
-7
View File
@@ -84,8 +84,6 @@ public:
// should be called every time new data is pushed into the filter
bool update();
static uint8_t getNumberOfStates() { return State::size; }
const StateSample &state() const { return _state; }
#if defined(CONFIG_EKF2_BAROMETER)
@@ -233,9 +231,6 @@ public:
const auto &aid_src_gravity() const { return _aid_src_gravity; }
#endif // CONFIG_EKF2_GRAVITY_FUSION
// get the state vector at the delayed time horizon
const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
#if defined(CONFIG_EKF2_WIND)
// get the wind velocity in m/s
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
@@ -408,8 +403,6 @@ public:
// return a bitmask integer that describes which state estimates can be used for flight control
void get_ekf_soln_status(uint16_t *status) const;
// rotate quaternion covariances into variances for an equivalent rotation vector
Vector3f calcRotVecVariances() const;
float getYawVar() const;
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }