fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: consolidate covariance corrections

Browse Source 
Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.
This commit is contained in:
Paul Riseborough
2016-05-07 20:29:50 +10:00
parent 0c6a367e1b
commit 7f5669fb2d
7 changed files with 62 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+3 -6
View File
@@ -362,8 +362,7 @@ void Ekf::fuseMag()
}
// correct the covariance marix for gross errors
makeSymmetrical();
limitCov();
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, _mag_innov[index]);
@@ -637,8 +636,7 @@ void Ekf::fuseHeading()
}
// correct the covariance marix for gross errors
makeSymmetrical();
limitCov();
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, _heading_innov);
@@ -772,8 +770,7 @@ void Ekf::fuseDeclination()
}
// correct the covariance marix for gross errors
makeSymmetrical();
limitCov();
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, innovation);