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

ekf2-mag: always add process noise until initial value

Browse Source
This commit is contained in:
bresch
2024-05-31 10:38:17 -04:00
committed by Daniel Agar
parent 493c9e49db
commit c11c75d32e
1 changed files with 15 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+15 -11
View File
@@ -163,22 +163,26 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag) { // mag_I: add process noise
// mag_I: add process noise float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); float mag_I_process_noise = sq(mag_I_sig);
float mag_I_process_noise = sq(mag_I_sig);
for (unsigned index = 0; index < State::mag_I.dof; index++) { for (unsigned index = 0; index < State::mag_I.dof; index++) {
const unsigned i = State::mag_I.idx + index; const unsigned i = State::mag_I.idx + index;
if (P(i, i) < sq(_params.mag_noise)) {
P(i, i) += mag_I_process_noise; P(i, i) += mag_I_process_noise;
} }
}
// mag_B: add process noise // mag_B: add process noise
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
float mag_B_process_noise = sq(mag_B_sig); float mag_B_process_noise = sq(mag_B_sig);
for (unsigned index = 0; index < State::mag_B.dof; index++) { for (unsigned index = 0; index < State::mag_B.dof; index++) {
const unsigned i = State::mag_B.idx + index; const unsigned i = State::mag_B.idx + index;
if (P(i, i) < sq(_params.mag_noise)) {
P(i, i) += mag_B_process_noise; P(i, i) += mag_B_process_noise;
} }
} }