From 16ae75a6810e4fb4363605d2b5cc298041317b64 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 17 Sep 2020 08:10:10 +1000 Subject: [PATCH] AP_NavEKF3: Consolidate and log tilt error variance calculation --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3.h | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 8 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 4 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 84 ++++++++++--------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 7 +- 7 files changed, 58 insertions(+), 52 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index a000cb1aa6..9fe5bd0b6c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1141,7 +1141,7 @@ void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias) const } } -// return tilt error convergence metric for the specified instance +// return estimated 1-sigma tilt error for the specified instance in radians void NavEKF3::getTiltError(int8_t instance, float &ang) const { if (instance < 0 || instance >= num_cores) instance = primary; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 3d06a0bf5d..cc9931d158 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -110,7 +110,7 @@ public: // An out of range instance (eg -1) returns data for the primary instance void getAccelBias(int8_t instance, Vector3f &accelBias) const; - // return tilt error convergence metric for the specified instance + // return estimated 1-sigma tilt error for the specified instance in radians // An out of range instance (eg -1) returns data for the primary instance void getTiltError(int8_t instance, float &ang) const; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index a0294a776d..2febe84b73 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -411,8 +411,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus() // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states // and declare the tilt alignment complete if (!tiltAlignComplete) { - Vector3f angleErrVarVec = calcRotVecVariances(); - if ((angleErrVarVec.x + angleErrVarVec.y) < sq(0.05235f)) { + if (tiltErrorVariance < sq(radians(3.0f))) { tiltAlignComplete = true; gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 9827317a17..4f3b319fab 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -190,8 +190,8 @@ void NavEKF3_core::alignYawAngle() } // set the yaw angle variance reflect the yaw sensor single sample uncertainty in yaw - // TODO get alignment uncertainty rel to gravity vector and use to set horizontal variances - Vector3f angleErrVarVec = Vector3f(sq(radians(3.0f)),sq(radians(3.0f)),sq(yawAngDataDelayed.yawAngErr)); + // assume tilt uncertainty split equally between roll and pitch + Vector3f angleErrVarVec = Vector3f(0.5f * tiltErrorVariance, 0.5f * tiltErrorVariance, sq(yawAngDataDelayed.yawAngErr)); CovariancePrediction(&angleErrVarVec); // send yaw alignment information to console @@ -1558,8 +1558,8 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationO Quaternion quat_delta = stateStruct.quat / quatBeforeReset; StoreQuatRotate(quat_delta); - // TODO get alignment uncertainty rel to gravity vector and use to set horizontal variances - Vector3f angleErrVarVec = Vector3f(sq(radians(3.0f)),sq(radians(3.0f)),yawVariance); + // assume tilt uncertainty split equally between roll and pitch + Vector3f angleErrVarVec = Vector3f(0.5f * tiltErrorVariance, 0.5f * tiltErrorVariance, yawVariance); CovariancePrediction(&angleErrVarVec); // record the yaw reset event diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index e17ef8b223..47c93c82c0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -169,10 +169,10 @@ void NavEKF3_core::getAccelBias(Vector3f &accelBias) const accelBias = stateStruct.accel_bias / dtEkfAvg; } -// return tilt error convergence metric +// return estimated 1-sigma tilt error in radians void NavEKF3_core::getTiltError(float &ang) const { - ang = stateStruct.quat.length(); + ang = sqrtf(MAX(tiltErrorVariance,0.0f)); } // return the transformation matrix from XYZ (body) to NED axes diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index a3653f9526..e9dc2de30b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1317,6 +1317,7 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) P[row][column] = P[column][row] = nextP[column][row]; } } + calcTiltErrorVariance(); return; } @@ -1659,6 +1660,8 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) // constrain values to prevent ill-conditioning ConstrainVariances(); + calcTiltErrorVariance(); + hal.util->perf_end(_perf_CovariancePrediction); } @@ -1938,47 +1941,50 @@ void NavEKF3_core::zeroAttCovOnly() } } -// calculate the variances for the rotation vector equivalent -Vector3f NavEKF3_core::calcRotVecVariances() +// calculate the tilt error variance +void NavEKF3_core::calcTiltErrorVariance() { - Vector3f rotVarVec; - float q0 = stateStruct.quat[0]; - float q1 = stateStruct.quat[1]; - float q2 = stateStruct.quat[2]; - float q3 = stateStruct.quat[3]; - if (q0 < 0) { - q0 = -q0; - q1 = -q1; - q2 = -q2; - q3 = -q3; - } - float t2 = q0*q0; - float t3 = acosf(q0); - float t4 = -t2+1.0f; - float t5 = t2-1.0f; - if ((t4 > 1e-9f) && (t5 < -1e-9f)) { - float t6 = 1.0f/t5; - float t7 = q1*t6*2.0f; - float t8 = 1.0f/powf(t4,1.5f); - float t9 = q0*q1*t3*t8*2.0f; - float t10 = t7+t9; - float t11 = 1.0f/sqrtf(t4); - float t12 = q2*t6*2.0f; - float t13 = q0*q2*t3*t8*2.0f; - float t14 = t12+t13; - float t15 = q3*t6*2.0f; - float t16 = q0*q3*t3*t8*2.0f; - float t17 = t15+t16; - rotVarVec.x = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; - rotVarVec.y = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; - rotVarVec.z = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; - } else { - rotVarVec.x = 4.0f * P[1][1]; - rotVarVec.y = 4.0f * P[2][2]; - rotVarVec.z = 4.0f * P[3][3]; - } + const float &q0 = stateStruct.quat[0]; + const float &q1 = stateStruct.quat[1]; + const float &q2 = stateStruct.quat[2]; + const float &q3 = stateStruct.quat[3]; - return rotVarVec; + // equations generated by quaternion_error_propagation(): in AP_NavEKF3/derivation/main.py + // only diagonals have been used + // dq0 ... dq3 terms have been zeroed + const float PS1 = q0*q1 + q2*q3; + const float PS2 = q1*PS1; + const float PS4 = sq(q0) - sq(q1) - sq(q2) + sq(q3); + const float PS5 = q0*PS4; + const float PS6 = 2*PS2 + PS5; + const float PS8 = PS1*q2; + const float PS10 = PS4*q3; + const float PS11 = PS10 + 2*PS8; + const float PS12 = PS1*q3; + const float PS13 = PS4*q2; + const float PS14 = -2*PS12 + PS13; + const float PS15 = PS1*q0; + const float PS16 = q1*PS4; + const float PS17 = 2*PS15 - PS16; + const float PS18 = q0*q2 - q1*q3; + const float PS19 = PS18*q2; + const float PS20 = 2*PS19 + PS5; + const float PS22 = q1*PS18; + const float PS23 = -PS10 + 2*PS22; + const float PS25 = PS18*q3; + const float PS26 = PS16 + 2*PS25; + const float PS28 = PS18*q0; + const float PS29 = -PS13 + 2*PS28; + const float PS32 = PS12 + PS28; + const float PS33 = PS19 + PS2; + const float PS34 = PS15 - PS25; + const float PS35 = PS22 - PS8; + + tiltErrorVariance = 4*sq(PS11)*P[2][2] + 4*sq(PS14)*P[3][3] + 4*sq(PS17)*P[0][0] + 4*sq(PS6)*P[1][1]; + tiltErrorVariance += 4*sq(PS20)*P[2][2] + 4*sq(PS23)*P[1][1] + 4*sq(PS26)*P[3][3] + 4*sq(PS29)*P[0][0]; + tiltErrorVariance += 16*sq(PS32)*P[1][1] + 16*sq(PS33)*P[3][3] + 16*sq(PS34)*P[2][2] + 16*sq(PS35)*P[0][0]; + + tiltErrorVariance = constrain_float(tiltErrorVariance, 0.0f, sq(radians(30.0f))); } void NavEKF3_core::bestRotationOrder(rotationOrder &order) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c5c157e114..fcf0d1d900 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -139,7 +139,7 @@ public: // return accelerometer bias in m/s/s void getAccelBias(Vector3f &accelBias) const; - // return tilt error convergence metric + // return estimated 1-sigma tilt error in radians void getTiltError(float &ang) const; // reset body axis gyro bias estimates @@ -942,8 +942,8 @@ private: // effective value of MAG_CAL MagCal effective_magCal(void) const; - // calculate the variances for the rotation vector equivalent - Vector3f calcRotVecVariances(void); + // calculate the tilt error variance) + void calcTiltErrorVariance(void); // update timing statistics structure void updateTimingStatistics(void); @@ -1149,6 +1149,7 @@ private: Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m) uint32_t firstInitTime_ms; // First time the initialise function was called (msec) uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec) + float tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2) // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position struct {