ekf2: make filter update period configurable

- introduces new parameter EKF2_PREDICT_US to configure the filter
update period in microseconds
 - actual filter update period will be an integer multiple of IMU
This commit is contained in:
Daniel Agar
2022-01-26 10:57:09 -05:00
parent 3f25349eb9
commit ae3070bbf1
20 changed files with 856 additions and 821 deletions
+3 -3
View File
@@ -56,7 +56,7 @@ void Ekf::initialiseCovariance()
_delta_angle_bias_var_accum.setZero();
_delta_vel_bias_var_accum.setZero();
const float dt = FILTER_UPDATE_PERIOD_S;
const float dt = _dt_ekf_avg;
resetQuatCov();
@@ -122,8 +122,8 @@ void Ekf::predictCovariance()
const float &dvz_b = _state.delta_vel_bias(2);
// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
const float dt = FILTER_UPDATE_PERIOD_S;
const float dt_inv = 1.0f / dt;
const float dt = _dt_ekf_avg;
const float dt_inv = 1.f / dt;
// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);