Browse Source

AP_NavEKF2: Adjust attitude variance values used after a heading reset

Because these errors are in body frame, a spherical error should be used to accommodate different orientations
mission-4.1.18
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
commit
581c1aa0d4
  1. 3
      libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

3
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@ -108,8 +108,7 @@ void NavEKF2_core::realignYawGPS() @@ -108,8 +108,7 @@ void NavEKF2_core::realignYawGPS()
zeroRows(P,0,2);
// Set the initial attitude error covariances
P[1][1] = P[0][0] = sq(radians(5.0f));
P[2][2] = sq(radians(45.0f));
P[2][2] = P[1][1] = P[0][0] = sq(radians(5.0f));
// reset tposition fusion timer to cause the states to be reset to the GPS on the next GPS fusion cycle
lastPosPassTime_ms = 0;

Loading…
Cancel
Save