Merge pull request #2679 from PX4/ekf_coning_fix

Fix coning handling in att_pos EKF. Fixes #2663
This commit is contained in:
Lorenz Meier 2015-08-14 14:14:38 +02:00
commit a12c1def63
2 changed files with 5 additions and 4 deletions

View File

@ -96,6 +96,7 @@ AttPosEKF::AttPosEKF() :
correctedDelVel(),
summedDelAng(),
summedDelVel(),
prevDelAng(),
accNavMag(),
earthRateNED(),
angRate(),
@ -273,12 +274,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
delAngTotal.y += correctedDelAng.y;
delAngTotal.z += correctedDelAng.z;
// Save current measurements
Vector3f prevDelAng = correctedDelAng;
// Apply corrections for earths rotation rate and coning errors
// * and + operators have been overloaded
correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
prevDelAng = correctedDelAng;
// Convert the rotation vector to its equivalent quaternion
rotationMag = correctedDelAng.length();
@ -3290,6 +3289,7 @@ void AttPosEKF::ZeroVariables()
correctedDelAng.zero();
summedDelAng.zero();
summedDelVel.zero();
prevDelAng.zero();
dAngIMU.zero();
dVelIMU.zero();
lastGyroOffset.zero();

View File

@ -139,6 +139,7 @@ public:
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f prevDelAng; ///< previous delta angle, used for coning correction
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)