clean up

src/modules/ekf2/CMakeLists.txt

@@ -241,7 +241,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 		-fno-associative-math
-		-DDEBUG_BUILD
+		#-DDEBUG_BUILD
 		#-O0
 	INCLUDES
 		EKF

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

@@ -46,13 +46,12 @@ void RangeFinderConsistencyCheck::init(const float z, const float z_var, const f
 {
 	printf("RangeFinderConsistencyCheck::init\n");
 
-	_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
+	_p = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
-	_Ht = sym::RangeValidationFilterH<float>();
+	_ht = sym::RangeValidationFilterH<float>();
 
 	_x(RangeFilter::z.idx) = z;
 	_x(RangeFilter::terrain.idx) = z - dist_bottom;
 	_initialized = true;
-	// _test_ratio_lpf.reset(0.f);
 	_test_ratio = 0.f;
 	_t_since_first_sample = 0.f;
 }
@@ -75,8 +74,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 	// prediction step
 	_time_last_update_us = time_us;
 	_x(RangeFilter::z.idx) -= dt * vz;
-	_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
+	_p(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
-	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
+	_p(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
 
 	// iterate through both measurements (z and dist_bottom)
 	const Vector2f measurements{z, dist_bottom};
@@ -84,7 +83,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
 
 		// dist_bottom
-		Vector2f H = _Ht;
+		Vector2f H = _ht;
 		float R = dist_bottom_var;
 
 		// z, direct state measurement
@@ -101,8 +100,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 		// for H as col-vector:
 		// innovation variance S = H^T * P * H + R
 		// kalman gain K = P * H / S
-		const float S = (H.transpose() * _P * H + R)(0, 0);
+		const float S = (H.transpose() * _p * H + R)(0, 0);
-		Vector2f K = (_P * H / S);
+		Vector2f K = (_p * H / S);
 
 		if (measurement_idx == 0) {
 			K(RangeFilter::z.idx) = 1.f;
@@ -110,8 +109,6 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 		} else if (measurement_idx == 1) {
 			_innov = y;
 			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
-			// _test_ratio_lpf.setParameters(dt, _t_to_init);
-			// _test_ratio_lpf.update(sign(_innov) * test_ratio);
 			_test_ratio = sign(_innov) * test_ratio;
 		}
 
@@ -124,7 +121,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
 		Matrix2f I;
 		I.setIdentity();
 		Matrix2f IKH = I - K.multiplyByTranspose(H);
-		_P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K);
+		_p = IKH * _p * IKH.transpose() + (K * R).multiplyByTranspose(K);
 	}
 
 	evaluateState(dt, vz, vz_var);

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp

@@ -40,7 +40,9 @@
 #ifndef EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 #define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 
-#include <mathlib/math/filter/AlphaFilter.hpp>
+#include <mathlib/math/Functions.hpp>
+
+using namespace math;
 
 class RangeFinderConsistencyCheck final
 {
@@ -54,7 +56,6 @@ public:
 		UNKNOWN = 2
 	};
 
-	// float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
 	float getTestRatioLpf() const { return _test_ratio; }
 
 	float getInnov() const { return _initialized ? _innov : 0.f; }
@@ -75,15 +76,14 @@ private:
 	void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
 	void evaluateState(float dt, float vz, float vz_var);
 	float _terrain_process_noise{0.0f};
-	matrix::SquareMatrix<float, 2> _P{};
+	matrix::SquareMatrix<float, 2> _p{};
-	matrix::Vector2f _Ht{};
+	matrix::Vector2f _ht{};
 	matrix::Vector2f _x{};
 	bool _initialized{false};
 	float _innov{0.f};
 	float _innov_var{0.f};
 	uint64_t _time_last_update_us{0};
 	static constexpr float time_constant{1.f};
-	// AlphaFilter<float> _test_ratio_lpf{time_constant};
 
 	float _test_ratio{0.f};
 

src/modules/ekf2/EKF/covariance.cpp

@@ -105,8 +105,7 @@ void Ekf::initialiseCovariance()
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	// use the ground clearance value as our uncertainty
+	// use the ground clearance value as our uncertainty. TODO: wouldn't it make more sense to  use the noise value instead?
-	// TODO: ^ why? wouldn't the noise value make more sense?
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_rng_noise));
 #endif // CONFIG_EKF2_TERRAIN
 }
@@ -230,7 +229,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-		// TODO: explain
 		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
 #endif // CONFIG_EKF2_RANGE_FINDER
 	}