dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix coning handling in att_pos EKF. Fixes #2663

This commit is contained in:
Lorenz Meier 2015-08-12 12:29:42 +02:00
parent cbada5198d
commit 3fd0ddf1b0
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -93,6 +93,7 @@ AttPosEKF::AttPosEKF() :
correctedDelVel(), correctedDelVel(),
summedDelAng(), summedDelAng(),
summedDelVel(), summedDelVel(),
prevDelAng(),
accNavMag(), accNavMag(),
earthRateNED(), earthRateNED(),
angRate(), angRate(),
@ -270,12 +271,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
delAngTotal.y += correctedDelAng.y; delAngTotal.y += correctedDelAng.y;
delAngTotal.z += correctedDelAng.z; delAngTotal.z += correctedDelAng.z;
// Save current measurements
Vector3f prevDelAng = correctedDelAng;
// Apply corrections for earths rotation rate and coning errors // Apply corrections for earths rotation rate and coning errors
// * and + operators have been overloaded // * 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 // Convert the rotation vector to its equivalent quaternion
rotationMag = correctedDelAng.length(); rotationMag = correctedDelAng.length();
@ -3287,6 +3286,7 @@ void AttPosEKF::ZeroVariables()
correctedDelAng.zero(); correctedDelAng.zero();
summedDelAng.zero(); summedDelAng.zero();
summedDelVel.zero(); summedDelVel.zero();
prevDelAng.zero();
dAngIMU.zero(); dAngIMU.zero();
dVelIMU.zero(); dVelIMU.zero();
lastGyroOffset.zero(); lastGyroOffset.zero();

1
src/modules/ekf_att_pos_estimator/estimator_22states.h
View File

@ -139,6 +139,7 @@ public:
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) 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 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 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) 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 earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)