From 8a031677d5958651bc10c66f4756caa3a156240a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 11 Jan 2024 11:53:20 -0500
Subject: [PATCH] ekf2: verbose debug print status include state variances

---
 src/modules/ekf2/EKF/ekf.cpp | 70 ++++++++++++++++++++++++------------
 1 file changed, 47 insertions(+), 23 deletions(-)

diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index 213f157f4a26..20c7536853af 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -379,34 +379,58 @@ static void printRingBuffer(const char *name, RingBuffer<T> *rb)
 void Ekf::print_status()
 {
 	printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6);
-	printf("Orientation: [%.4f, %.4f, %.4f, %.4f] (Euler [%.3f, %.3f, %.3f])\n",
-	       (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2),
-	       (double)_state.quat_nominal(3),
-	       (double)matrix::Eulerf(_state.quat_nominal).phi(), (double)matrix::Eulerf(_state.quat_nominal).theta(),
-	       (double)matrix::Eulerf(_state.quat_nominal).psi());
-
-	printf("Velocity: [%.3f, %.3f, %.3f]\n",
-	       (double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2));
-
-	printf("Position: [%.3f, %.3f, %.3f]\n",
-	       (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2));
-
-	printf("Gyro Bias: [%.6f, %.6f, %.6f]\n",
-	       (double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2));
-
-	printf("Accel Bias: [%.6f, %.6f, %.6f]\n",
-	       (double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2));
+	printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n",
+	       State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1,
+	       (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3),
+	       (double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()),
+	       (double)getStateVariance<State::quat_nominal>()(0), (double)getStateVariance<State::quat_nominal>()(1), (double)getStateVariance<State::quat_nominal>()(2)
+	      );
+
+	printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
+	       State::vel.idx, State::vel.idx + State::vel.dof - 1,
+	       (double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2),
+	       (double)getStateVariance<State::vel>()(0), (double)getStateVariance<State::vel>()(1), (double)getStateVariance<State::vel>()(2)
+	      );
+
+	printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
+	       State::pos.idx, State::pos.idx + State::pos.dof - 1,
+	       (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
+	       (double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1), (double)getStateVariance<State::pos>()(2)
+	      );
+
+	printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
+	       State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1,
+	       (double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2),
+	       (double)getStateVariance<State::gyro_bias>()(0), (double)getStateVariance<State::gyro_bias>()(1), (double)getStateVariance<State::gyro_bias>()(2)
+	      );
+
+	printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
+	       State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1,
+	       (double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2),
+	       (double)getStateVariance<State::accel_bias>()(0), (double)getStateVariance<State::accel_bias>()(1), (double)getStateVariance<State::accel_bias>()(2)
+	      );
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	printf("Magnetic Field: [%.3f, %.3f, %.3f]\n",
-	       (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
-
-	printf("Magnetic Bias: [%.3f, %.3f, %.3f]\n",
-	       (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
+	printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
+	       State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1,
+	       (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
+	       (double)getStateVariance<State::mag_I>()(0), (double)getStateVariance<State::mag_I>()(1), (double)getStateVariance<State::mag_I>()(2)
+	      );
+
+	printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
+	       State::mag_B.idx, State::mag_B.idx + State::mag_B.dof - 1,
+	       (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2),
+	       (double)getStateVariance<State::mag_B>()(0), (double)getStateVariance<State::mag_B>()(1),
+	       (double)getStateVariance<State::mag_B>()(2)
+	      );
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)
-	printf("Wind velocity: [%.3f, %.3f]\n", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
+	printf("Wind velocity (%d-%d): [%.3f, %.3f] var: [%.1e, %.1e]\n",
+	       State::wind_vel.idx, State::wind_vel.idx + State::wind_vel.dof - 1,
+	       (double)_state.wind_vel(0), (double)_state.wind_vel(1),
+	       (double)getStateVariance<State::wind_vel>()(0), (double)getStateVariance<State::wind_vel>()(1)
+	      );
 #endif // CONFIG_EKF2_WIND
 
 	printf("\nP:\n");