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

ekf2: refactor output predictor to class

Browse Source 
- refactor all EKF backend output predictor pieces into new OutputPredictor class
 - output states are now calculated immediately with new high rate IMU rather than after EKF update
 - IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear
This commit is contained in:
Daniel Agar
2023-01-18 10:59:34 -05:00
committed by GitHub
parent c054ca20cc
commit 95300d5637
39 changed files with 855 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+9 -9
View File
@@ -98,7 +98,7 @@ void Ekf::initialiseCovariance()
}
void Ekf::predictCovariance()
void Ekf::predictCovariance(const imuSample &imu_delayed)
{
// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
const float dt = _dt_ekf_avg;
@@ -114,9 +114,9 @@ void Ekf::predictCovariance()
// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
const float beta = 1.0f - alpha;
_ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
_accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;
_ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
_accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
_accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt;
const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
@@ -141,7 +141,7 @@ void Ekf::predictCovariance()
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index] || !is_bias_observable;
const bool do_inhibit_axis = do_inhibit_all_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
@@ -220,7 +220,7 @@ void Ekf::predictCovariance()
Vector3f d_vel_var;
for (int i = 0; i <= 2; i++) {
if (_fault_status.flags.bad_acc_vertical || _imu_sample_delayed.delta_vel_clipping[i]) {
if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) {
// Increase accelerometer process noise if bad accel data is detected
d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE);
@@ -233,7 +233,7 @@ void Ekf::predictCovariance()
SquareMatrix24f nextP;
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, _imu_sample_delayed.delta_vel, d_vel_var, _imu_sample_delayed.delta_ang, d_ang_var, dt, &nextP);
sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP);
// process noise contribution for delta angle states can be very small compared to
// the variances, therefore use algorithm to minimise numerical error
@@ -423,7 +423,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _imu_sample_delayed.time_us;
_time_acc_bias_check = _time_delayed_us;
} else {
_fault_status.flags.bad_acc_bias = true;
@@ -435,7 +435,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
P.uncorrelateCovariance<3>(13);
_time_acc_bias_check = _imu_sample_delayed.time_us;
_time_acc_bias_check = _time_delayed_us;
_fault_status.flags.bad_acc_bias = false;
_warning_events.flags.invalid_accel_bias_cov_reset = true;
ECL_WARN("invalid accel bias - covariance reset");