ekf2: treat inhibited states as "consider states" and remove conservative covariance matrix fixes (#22597)

When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2024-01-24 14:14:09 -05:00
committed by GitHub
parent 2c81c9fdea
commit 51155f7a29
13 changed files with 874 additions and 1039 deletions
+3 -5
View File
@@ -89,9 +89,6 @@ void Ekf::reset()
_fault_status.value = 0;
_innov_check_fail_status.value = 0;
_prev_gyro_bias_var.zero();
_prev_accel_bias_var.zero();
#if defined(CONFIG_EKF2_GNSS)
resetGpsDriftCheckFilters();
_gps_checks_passed = false;
@@ -312,8 +309,9 @@ void Ekf::predictState(const imuSample &imu_delayed)
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
constrainStates();
// constrain states
_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate