dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: fix mag and wind covariance prediction

This commit is contained in:
bresch 2024-03-07 15:03:59 +01:00 committed by Daniel Agar
parent 2bacb4b65d
commit 0639f5370c
1 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf2/EKF/covariance.cpp
View File

@ -166,11 +166,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
if (_control_status.flags.mag) {
// mag_I: add process noise
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
P.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) += sq(mag_I_sig);
float mag_I_process_noise = sq(mag_I_sig);
for (unsigned index = 0; index < State::mag_I.dof; index++) {
const unsigned i = State::mag_I.idx + index;
P(i, i) += mag_I_process_noise;
}
// mag_B: add process noise
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
P.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) += sq(mag_B_sig);
float mag_B_process_noise = sq(mag_B_sig);
for (unsigned index = 0; index < State::mag_B.dof; index++) {
const unsigned i = State::mag_B.idx + index;
P(i, i) += mag_B_process_noise;
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
@ -179,7 +189,12 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
if (_control_status.flags.wind) {
// wind vel: add process noise
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
P.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) += sq(wind_vel_nsd_scaled) * dt;
float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
const unsigned i = State::wind_vel.idx + index;
P(i, i) += wind_vel_process_noise;
}
}
#endif // CONFIG_EKF2_WIND