ekf: add logging of range finder consistency check

src/modules/ekf2/EKF/ekf.h

@@ -128,6 +128,10 @@ public:
 	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
 	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
 
+	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
+	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
+	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
+
 	// get the state vector at the delayed time horizon
 	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
 

src/modules/ekf2/EKF/range_finder_consistency_check.cpp

@@ -63,6 +63,10 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 
 	_time_last_update_us = time_us;
 	_dist_bottom_prev = dist_bottom;
+
+	// Save for logging
+	_vel_bottom_innov = innov;
+	_vel_bottom_innov_var = innov_var;
 }
 
 void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -51,6 +51,8 @@ public:
 
 	float getTestRatio() const { return _vel_bottom_test_ratio; }
 	float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); }
+	float getInnov() const { return _vel_bottom_innov; }
+	float getInnovVar() const { return _vel_bottom_innov_var; }
 	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
 
 private:
@@ -61,6 +63,8 @@ private:
 
 	float _vel_bottom_test_ratio{};
 	AlphaFilter<float> _vel_bottom_signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
+	float _vel_bottom_innov{};
+	float _vel_bottom_innov_var{};
 
 	bool _is_kinematically_consistent{true};
 	uint64_t _time_last_inconsistent_us{};