ekf2: rename BaroBiasEstimator -> BiasEstimator

src/modules/ekf2/CMakeLists.txt

@@ -48,7 +48,7 @@ px4_add_module(
 		3600
 	SRCS
 		EKF/airspeed_fusion.cpp
-		EKF/baro_bias_estimator.cpp
+		EKF/bias_estimator.cpp
 		EKF/control.cpp
 		EKF/covariance.cpp
 		EKF/drag_fusion.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -33,7 +33,7 @@
 
 add_library(ecl_EKF
 	airspeed_fusion.cpp
-	baro_bias_estimator.cpp
+	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
 	drag_fusion.cpp

src/modules/ekf2/EKF/bias_estimator.cpp

@@ -32,14 +32,14 @@
  ****************************************************************************/
 
 /**
- * @file baro_bias_estimator.cpp
+ * @file bias_estimator.cpp
  *
  * @author Mathieu Bresciani 	<mathieu@auterion.com>
  */
 
-#include "baro_bias_estimator.hpp"
+#include "bias_estimator.hpp"
 
-void BaroBiasEstimator::predict(const float dt)
+void BiasEstimator::predict(const float dt)
 {
 	// State is constant
 	// Predict state covariance only
@@ -62,12 +62,12 @@ void BaroBiasEstimator::predict(const float dt)
 	_status.bias_var = _state_var;
 }
 
-void BaroBiasEstimator::constrainStateVar()
+void BiasEstimator::constrainStateVar()
 {
 	_state_var = math::constrain(_state_var, 1e-8f, _state_var_max);
 }
 
-void BaroBiasEstimator::fuseBias(const float measurement, const float measurement_var)
+void BiasEstimator::fuseBias(const float measurement, const float measurement_var)
 {
 	const float innov_var = _state_var + measurement_var;
 	const float innov = measurement - _state;
@@ -85,28 +85,28 @@ void BaroBiasEstimator::fuseBias(const float measurement, const float measuremen
 	_status = packStatus(innov, innov_var, innov_test_ratio);
 }
 
-inline float BaroBiasEstimator::computeInnovTestRatio(const float innov, const float innov_var) const
+inline float BiasEstimator::computeInnovTestRatio(const float innov, const float innov_var) const
 {
 	return innov * innov / (_gate_size * _gate_size * innov_var);
 }
 
-inline bool BaroBiasEstimator::isTestRatioPassing(const float innov_test_ratio) const
+inline bool BiasEstimator::isTestRatioPassing(const float innov_test_ratio) const
 {
 	return innov_test_ratio < 1.f;
 }
 
-inline void BaroBiasEstimator::updateState(const float K, const float innov)
+inline void BiasEstimator::updateState(const float K, const float innov)
 {
 	_state = math::constrain(_state + K * innov, -_state_max, _state_max);
 }
 
-inline void BaroBiasEstimator::updateStateCovariance(const float K)
+inline void BiasEstimator::updateStateCovariance(const float K)
 {
 	_state_var -= K * _state_var;
 	constrainStateVar();
 }
 
-inline void BaroBiasEstimator::updateOffsetDetection(const float innov, const float innov_test_ratio)
+inline void BiasEstimator::updateOffsetDetection(const float innov, const float innov_test_ratio)
 {
 	const float signed_innov_test_ratio = matrix::sign(innov) * innov_test_ratio;
 	_signed_innov_test_ratio_lpf.update(math::constrain(signed_innov_test_ratio, -1.f, 1.f));
@@ -121,7 +121,7 @@ inline void BaroBiasEstimator::updateOffsetDetection(const float innov, const fl
 	}
 }
 
-inline bool BaroBiasEstimator::isOffsetDetected() const
+inline bool BiasEstimator::isOffsetDetected() const
 {
 	// There is an offset in the estimate if the average of innovation is statistically too large
 	// or if the sign of the innovation is constantly the same
@@ -130,7 +130,7 @@ inline bool BaroBiasEstimator::isOffsetDetected() const
 	       || (_time_since_last_negative_innov > _innov_sequence_monitnoring_time_constant);
 }
 
-inline BaroBiasEstimator::status BaroBiasEstimator::packStatus(const float innov, const float innov_var,
+inline BiasEstimator::status BiasEstimator::packStatus(const float innov, const float innov_var,
 		const float innov_test_ratio) const
 {
 	// Send back status for logging

src/modules/ekf2/EKF/bias_estimator.hpp

@@ -32,32 +32,28 @@
  ****************************************************************************/
 
 /**
- * @file baro_bias_estimator.hpp
+ * @file bias_estimator.hpp
- * @brief Implementation of a single-state baro bias estimator
+ * @brief Implementation of a single-state bias estimator
  *
- * state: baro bias (in meters)
+ * state: bias
  *
  * The state is noise driven: Transition matrix A = 1
  * x[k+1] = Ax[k] + v with v ~ N(0, Q)
  * y[k] = x[k] + w with w ~ N(0, R)
  *
- * The difference between the current barometric altitude and another absolute
- * reference (e.g.: GNSS altitude) is used as a bias measurement.
- *
  * During the measurment update step, the Normalized Innovation Squared (NIS) is checked
  * and the measurement is rejected if larger than the gate size.
  *
  * @author Mathieu Bresciani 	<mathieu@auterion.com>
  */
 
-#ifndef EKF_BARO_BIAS_ESTIMATOR_HPP
+#pragma once
-#define EKF_BARO_BIAS_ESTIMATOR_HPP
 
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
 
-class BaroBiasEstimator
+class BiasEstimator
 {
 public:
 	struct status {
@@ -68,8 +64,8 @@ public:
 		float innov_test_ratio;
 	};
 
-	BaroBiasEstimator() = default;
+	BiasEstimator() = default;
-	~BaroBiasEstimator() = default;
+	~BiasEstimator() = default;
 
 	void predict(float dt);
 	void fuseBias(float measurement, float measurement_var);
@@ -127,4 +123,3 @@ private:
 	static constexpr float _innov_sequence_monitnoring_time_constant{10.f}; ///< in seconds
 	static constexpr float _process_var_boost_gain{1.0e3f};
 };
-#endif // !EKF_BARO_BIAS_ESTIMATOR_HPP

src/modules/ekf2/EKF/ekf.h

@@ -46,7 +46,7 @@
 #include "estimator_interface.h"
 
 #include "EKFGSF_yaw.h"
-#include "baro_bias_estimator.hpp"
+#include "bias_estimator.hpp"
 
 #include <uORB/topics/estimator_aid_source_1d.h>
 #include <uORB/topics/estimator_aid_source_2d.h>
@@ -345,7 +345,7 @@ public:
 	// Returns true if the output of the yaw emergency estimator can be used for a reset
 	bool isYawEmergencyEstimateAvailable() const;
 
-	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
+	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
 
@@ -1042,7 +1042,7 @@ private:
 	// yaw estimator instance
 	EKFGSF_yaw _yawEstimator{};
 
-	BaroBiasEstimator _baro_b_est{};
+	BiasEstimator _baro_b_est{};
 
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
 	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate

src/modules/ekf2/EKF2.cpp

@@ -680,7 +680,7 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
 {
 	if (_device_id_baro != 0) {
-		const BaroBiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
+		const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
 
 		if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) {
 			estimator_baro_bias_s baro_bias{};