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

EKF2 simplify covariance propagation (#22344)

Browse Source 
* ekf2-derivation: optimize before generating cpp code

* update EKF2 change indicator

Slight changes due to simplifications done in the covariance prediction

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
This commit is contained in:
Mathieu Bresciani
2023-11-14 11:22:16 -05:00
committed by GitHub
co-authored by bresch
parent 96e05481b4
commit efa12ad224
5 changed files with 571 additions and 757 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+7 -6
View File
@@ -108,27 +108,28 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// delta angle noise variance
float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise);
const float gyro_var = sq(gyro_noise);
// delta velocity noise variance
float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
Vector3f d_vel_var;
Vector3f accel_var;
for (unsigned i = 0; i < 3; 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(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE);
accel_var(i) = sq(BADACC_BIAS_PNOISE);
} else {
d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise);
accel_var(i) = sq(accel_noise);
}
}
// predict the covariance
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
P = sym::PredictCovariance(_state.vector(), P,
imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var,
imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var);
imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt));
// Construct the process noise variance diagonal for those states with a stationary process model
// These are kinematic states and their error growth is controlled separately by the IMU noise variances