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

EKF2: centralized auto-generated state (#22183)

Browse Source 
* ekf2_derivation: use single source of state definition

The state is defined as an ordered dictionary of group elements and
everything else is generated using that state definition

* ekf2: generated state sample add const reference getter

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Mathieu Bresciani
2023-10-06 10:28:21 -04:00
committed by GitHub
co-authored by bresch Daniel Agar
parent 2e1d5687f9
commit 05fd8c5976
15 changed files with 279 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+2 -2
View File
@@ -144,7 +144,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
float innov = 0.f;
float innov_var = 0.f;
sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
aid_src.observation = airspeed_sample.true_airspeed;
aid_src.observation_variance = R;
@@ -195,7 +195,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
VectorState H; // Observation jacobian
VectorState K; // Kalman gain vector
sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);
sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K);
if (update_wind_only) {
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);