From 6c7820dd8dbf046648c35285dbab34d06042cc02 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Thu, 17 Sep 2020 10:44:58 +1000
Subject: [PATCH] AP_NavEKF3: Add verification of tiltErrorVariance() method in
 SITL

---
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 61 ++++++++++++++++++++++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h   |  8 +++-
 2 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index e9dc2de30b..6b65546e20 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -6,6 +6,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_GPS/AP_GPS.h>
 #include <AP_VisualOdom/AP_VisualOdom.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -1662,6 +1663,10 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr)
 
     calcTiltErrorVariance();
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    verifyTiltErrorVariance();
+#endif
+
     hal.util->perf_end(_perf_CovariancePrediction);
 }
 
@@ -1997,3 +2002,59 @@ void NavEKF3_core::bestRotationOrder(rotationOrder &order)
         order = rotationOrder::TAIT_BRYAN_312;
     }
 }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+// calculate the tilt error variance using an alternative numerical difference technique
+// and log with value generated by NavEKF3_core::calcTiltErrorVariance()
+void NavEKF3_core::verifyTiltErrorVariance()
+{
+    const Vector3f gravity_ef = Vector3f(0.0f,0.0f,1.0f);
+    Matrix3f Tnb;
+    const float quat_delta = 0.001f;
+    float tiltErrorVarianceAlt = 0.0f;
+    for (uint8_t index = 0; index<4; index++) {
+        Quaternion quat = stateStruct.quat;
+
+        // Add a positive increment to the quaternion element and calculate the tilt error vector
+        quat[index] = stateStruct.quat[index] + quat_delta;
+        quat.inverse().rotation_matrix(Tnb);
+        const Vector3f gravity_bf_plus = Tnb * gravity_ef;
+
+        // Add a negative increment to the quaternion element and calculate the tilt error vector
+        quat[index] = stateStruct.quat[index] - quat_delta;
+        quat.inverse().rotation_matrix(Tnb);
+        const Vector3f gravity_bf_minus = Tnb * gravity_ef;
+
+        // calculate the angular difference between the two vectors using a small angle assumption
+        const Vector3f tilt_diff_vec = gravity_bf_minus % gravity_bf_plus;
+
+        // calculate the partial derivative of angle error wrt the quaternion element
+        const float tilt_error_derivative = tilt_diff_vec.length() / (2.0f * quat_delta);
+
+        // sum the contribution of the quaternion elemnent variance to the tilt angle error variance
+        tiltErrorVarianceAlt += P[index][index] * sq(tilt_error_derivative);
+    }
+
+    tiltErrorVarianceAlt = MIN(tiltErrorVarianceAlt, sq(radians(30.0f)));
+
+    static uint32_t lastLogTime_ms = 0;
+    if (imuSampleTime_ms - lastLogTime_ms > 500) {
+        lastLogTime_ms = imuSampleTime_ms;
+        // @LoggerMessage: XKTV
+        // @Description: EKF3 Yaw Estimator States
+        // @Field: TimeUS: Time since system startup
+        // @Field: C: EKF3 core this data is for
+        // @Field: TVS: Tilt Error Variance from symboolic equations (rad^2)
+        // @Field: TVD: Tilt Error Variance from difference method (rad^2)
+        AP::logger().Write("XKTV",
+                        "TimeUS,C,TVS,TVD",
+                        "s#rr",
+                        "F-00",
+                        "QBff",
+                        AP_HAL::micros64(),
+                        core_index,
+                        tiltErrorVariance,
+                        tiltErrorVarianceAlt);
+    }
+}
+#endif
\ No newline at end of file
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index fcf0d1d900..514857e6ce 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -942,9 +942,15 @@ private:
     // effective value of MAG_CAL
     MagCal effective_magCal(void) const;
 
-    // calculate the tilt error variance)
+    // calculate the tilt error variance
     void calcTiltErrorVariance(void);
     
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // calculate the tilt error variance using an alternative numerical difference technique
+    // and log with value generated by NavEKF3_core::calcTiltErrorVariance()
+    void verifyTiltErrorVariance();
+#endif
+
     // update timing statistics structure
     void updateTimingStatistics(void);