ekf: add logging of range finder consistency check

msg/estimator_innovations.msg

@@ -31,6 +31,7 @@ float32[2] drag		# drag specific force innovation (m/sec**2) and innovation vari
 float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
 float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)
 float32 hagl		# height of ground innovation (m) and innovation variance (m**2)
+float32 hagl_rate	# height of ground rate innovation (m/s) and innovation variance ((m/s)**2)
 
 # The innovation test ratios are scalar values. In case the field is a vector,
 # the test ratio will be put in the first component of the vector.

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

src/modules/ekf2/EKF2.cpp

@@ -838,6 +838,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getAirspeedInnov(innovations.airspeed);
 	_ekf.getBetaInnov(innovations.beta);
 	_ekf.getHaglInnov(innovations.hagl);
+	_ekf.getHaglRateInnov(innovations.hagl_rate);
 	// Not yet supported
 	innovations.aux_vvel = NAN;
 
@@ -878,6 +879,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
 	_ekf.getBetaInnovRatio(test_ratios.beta);
 	_ekf.getHaglInnovRatio(test_ratios.hagl);
+	_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
 	// Not yet supported
 	test_ratios.aux_vvel = NAN;
 
@@ -902,6 +904,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getAirspeedInnovVar(variances.airspeed);
 	_ekf.getBetaInnovVar(variances.beta);
 	_ekf.getHaglInnovVar(variances.hagl);
+	_ekf.getHaglRateInnovVar(variances.hagl_rate);
 	// Not yet supported
 	variances.aux_vvel = NAN;