forked from Archive/PX4-Autopilot
ekf2: fix mag and wind covariance prediction
This commit is contained in:
parent
2bacb4b65d
commit
0639f5370c
|
@ -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
|
||||
|
||||
|
|
Loading…
Reference in New Issue