AP_NavEKF3: Consolidate and log tilt error variance calculation

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;

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;
 

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);
         }

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

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

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];
-    }
-
-    return rotVarVec;
+    const float &q0 = stateStruct.quat[0];
+    const float &q1 = stateStruct.quat[1];
+    const float &q2 = stateStruct.quat[2];
+    const float &q3 = stateStruct.quat[3];
+
+    // 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)

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 {