From 3fd0ddf1b058e423968ec5af34d03e38e38ce369 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Aug 2015 12:29:42 +0200 Subject: [PATCH] Fix coning handling in att_pos EKF. Fixes #2663 --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 8 ++++---- src/modules/ekf_att_pos_estimator/estimator_22states.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index bf70ed13bc..b920cb9458 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -93,6 +93,7 @@ AttPosEKF::AttPosEKF() : correctedDelVel(), summedDelAng(), summedDelVel(), + prevDelAng(), accNavMag(), earthRateNED(), angRate(), @@ -270,12 +271,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(); @@ -3287,6 +3286,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + prevDelAng.zero(); dAngIMU.zero(); dVelIMU.zero(); lastGyroOffset.zero(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 426340d2ce..6eb325d531 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -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)