A small problem in EKF2/3 in function NavEKF3_core::alignMagStateDeclination()

Development Team
1

Hello, I find a small problem in EKF2/3 in function NavEKF3_core::alignMagStateDeclination(), this function calls function NavEKF3_core::FuseDeclination(float declErr), the question is that FuseDeclination() here seems no useful. Because that in alignMagStateDeclination(), stateStruct.earth_magfield has already been set to:

stateStruct.earth_magfield.x = magLengthNE cosf(magDecAng);
stateStruct.earth_magfield.y = magLengthNE sinf(magDecAng);
So in FuseDeclination(), this peice os code eques to zero:

// Calculate the innovation
float innovation = atan2f(magE , magN) - magDecAng;
Why innovation is zero? Because here:

float magN = stateStruct.earth_magfield.x; = magLengthNE cosf(magDecAng);
float magE = stateStruct.earth_magfield.y; = magLengthNE sinf(magDecAng);
So atan2f(magE , magN) = magDecAng! Because both magDecAng in the two function are set by

float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
and called very closely, so the two magDecAng are supposed to the same. So the innovation is zero, so FuseDeclination() called here seems of no ues, the only use I think is to reduce the P (error between estimated value and true value).

2

As @priseborough has answered in the gitter, so I simply copy his answer to here:

Paul Riseborough @priseborough 05:49
@luweikxy The declination is fused after the alignment, not to change the states (you are correct there will be no change because the innovation is zero), but to condition the covariance matrix to prevent large variations in declination of the earth field states immediately after the alignment.