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

ekf2: centralize mag covariance reset (#693)

Browse Source 
* ekf2: centralize mag covariance reset. A complete and clean reset of the
mag states covariances is now performed through the "resetMagCov"
function only. This avoid having slight differences of implementations
across the code.

* Ekf: reset quat cov with initial uncertainty instead of zero
This commit is contained in:
Mathieu Bresciani
2020-03-17 20:07:20 +11:00
committed by GitHub
parent f1aa53db8a
commit f20fc08b7d
3 changed files with 39 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3 -33
View File
@@ -459,17 +459,7 @@ bool Ekf::realignYawGPS()
_R_to_earth = Dcmf(_state.quat_nominal);
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// reset the corresponding rows and columns in the covariance matrix and set the
// variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
}
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -485,17 +475,7 @@ bool Ekf::realignYawGPS()
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
}
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -579,17 +559,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// set the earth magnetic field states using the updated rotation
_state.mag_I = _R_to_earth * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
}
resetMagCov();
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;