mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-19 03:40:34 +08:00
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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user