EKF: Save mag field covariance information on startup

This commit is contained in:
Paul Riseborough 2019-01-17 16:52:20 +11:00 committed by Daniel Agar
parent 708c79eb8f
commit 82ce7a83a5
1 changed files with 3 additions and 0 deletions

View File

@ -106,6 +106,9 @@ void Ekf::initialiseCovariance()
P[index][index] = sq(_params.mag_noise); P[index][index] = sq(_params.mag_noise);
} }
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
// wind // wind
P[22][22] = sq(_params.initial_wind_uncertainty); P[22][22] = sq(_params.initial_wind_uncertainty);
P[23][23] = sq(_params.initial_wind_uncertainty); P[23][23] = sq(_params.initial_wind_uncertainty);