Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: reduce declination errors on start of 3-axis fusion

Browse Source 
Reset co-variances for NE field states.
This commit is contained in:
priseborough 2016-07-03 21:17:40 +10:00
parent 744f19cd2d
commit 136df7cb5c
2 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -231,13 +231,13 @@ void NavEKF2_core::SelectMagFusion()
// use the simple method of declination to maintain heading if we cannot use the magnetic field states
if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
fuseEulerYaw();
// zero the test ratio output from the inactive 3-axis magneteometer fusion
// zero the test ratio output from the inactive 3-axis magnetometer fusion
magTestRatio.zero();
} else {
// if we are not doing aiding with earth relative observations (eg GPS) then the declination is
// maintained by fusing declination as a synthesised observation
if (PV_AidingMode != AID_ABSOLUTE || (imuSampleTime_ms - lastPosPassTime_ms) > 4000) {
FuseDeclination();
FuseDeclination(0.34f);
}
// fuse the three magnetometer componenents sequentially
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
@ -935,10 +935,10 @@ void NavEKF2_core::fuseEulerYaw()
* This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS
* or some other absolute position or velocity reference
*/
void NavEKF2_core::FuseDeclination()
void NavEKF2_core::FuseDeclination(float declErr)
{
// declination error variance (rad^2)
const float R_DECL = 1e-2f;
const float R_DECL = sq(declErr);
// copy required states to local variables
float magN = stateStruct.earth_magfield.x;
@ -1070,6 +1070,17 @@ void NavEKF2_core::alignMagStateDeclination()
float magLengthNE = norm(initMagNED.x,initMagNED.y);
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
// zero the corresponding state covariances
float var_16 = P[16][16];
float var_17 = P[17][17];
zeroRows(P,16,17);
zeroCols(P,16,17);
P[16][16] = var_16;
P[17][17] = var_17;
// fuse the declination angle to re-establish valid covariances
FuseDeclination(0.1f);
}
// record a magentic field state reset event

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -603,7 +603,8 @@ private:
void fuseEulerYaw();
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
void FuseDeclination();
// Input is 1-sigma uncertainty in published declination
void FuseDeclination(float declErr);
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer