EKF2: Error-State Kalman Filter (#22262)

* ekf derivation: change to error state formulation
* ekf2: update auto-generated code for error-state
* ekf2: adjust ekf2 code for error state formulation
* ekf2_tests: adjust unit tests for error-state EKF
* update change indicator for error-state EKF
* ekf2_derivation: allow disabling mag and wind states

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>

msg/EstimatorStates.msg

@@ -4,4 +4,4 @@ uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 float32[24] states		# Internal filter states
 uint8 n_states		# Number of states effectively used
 
-float32[24] covariances	# Diagonal Elements of Covariance Matrix
+float32[23] covariances	# Diagonal Elements of Covariance Matrix

src/lib/matrix/matrix/Vector.hpp

@@ -153,6 +153,11 @@ class Vector : public Matrix<Type, M, 1>
 	{
 		(*this).transpose().print();
 	}
+
+	static size_t size()
+	{
+		return M;
+	}
 };
 
 template<typename OStream, typename Type, size_t M>

src/modules/ekf2/EKF/control.cpp

@@ -68,7 +68,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	// monitor the tilt alignment
 	if (!_control_status.flags.tilt_align) {
 		// whilst we are aligning the tilt, monitor the variances
-		const Vector3f angle_err_var_vec = calcRotVecVariances();
+		const Vector3f angle_err_var_vec = getQuaternionVariance();
 
 		// Once the tilt variances have reduced to equivalent of 3deg uncertainty
 		// and declare the tilt alignment complete

src/modules/ekf2/EKF/covariance.cpp

@@ -43,7 +43,6 @@
 
 #include "ekf.h"
 #include <ekf_derivation/generated/predict_covariance.h>
-#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
 
 #include <math.h>
 #include <mathlib/mathlib.h>
@@ -368,10 +367,8 @@ void Ekf::resetQuatCov(const float yaw_noise)
 
 void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
 {
-	matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
-	sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov);
-	q_cov.copyLowerToUpperTriangle();
-	resetStateCovariance<State::quat_nominal>(q_cov);
+	matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov_ned = diag(rot_var_ned);
+	resetStateCovariance<State::quat_nominal>(_R_to_earth.T() * q_cov_ned * _R_to_earth);
 }
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -84,7 +84,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 				      _state.vel(2));
 	const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth);
 	const float rel_wind_speed = rel_wind_body.norm();
-	const VectorState state_vector_prev = _state.vector();
+	const auto state_vector_prev = _state.vector();
 
 	Vector2f bcoef_inv{0.f, 0.f};
 

src/modules/ekf2/EKF/ekf.h

@@ -84,8 +84,6 @@ class Ekf final : public EstimatorInterface
 	// should be called every time new data is pushed into the filter
 	bool update();
 
-	static uint8_t getNumberOfStates() { return State::size; }
-
 	const StateSample &state() const { return _state; }
 
 #if defined(CONFIG_EKF2_BAROMETER)
@@ -233,9 +231,6 @@ class Ekf final : public EstimatorInterface
 	const auto &aid_src_gravity() const { return _aid_src_gravity; }
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 
-	// get the state vector at the delayed time horizon
-	const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
-
 #if defined(CONFIG_EKF2_WIND)
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
@@ -408,8 +403,6 @@ class Ekf final : public EstimatorInterface
 	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status) const;
 
-	// rotate quaternion covariances into variances for an equivalent rotation vector
-	Vector3f calcRotVecVariances() const;
 	float getYawVar() const;
 
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -40,8 +40,6 @@
  */
 
 #include "ekf.h"
-#include <ekf_derivation/generated/quat_var_to_rot_var.h>
-#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
 
 #include <mathlib/mathlib.h>
 #include <lib/world_magnetic_model/geo_mag_declination.h>
@@ -758,7 +756,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 
 void Ekf::fuse(const VectorState &K, float innovation)
 {
-	_state.quat_nominal -= K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * innovation;
+	Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * (-1.f * innovation)));
+	_state.quat_nominal *= delta_quat;
 	_state.quat_nominal.normalize();
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
@@ -851,21 +850,11 @@ void Ekf::updateVerticalDeadReckoningStatus()
 	}
 }
 
-// calculate the variances for the rotation vector equivalent
-Vector3f Ekf::calcRotVecVariances() const
-{
-	Vector3f rot_var;
-	sym::QuatVarToRotVar(_state.vector(), P, FLT_EPSILON, &rot_var);
-	return rot_var;
-}
-
 float Ekf::getYawVar() const
 {
-	VectorState H_YAW;
-	float yaw_var = 0.f;
-	computeYawInnovVarAndH(0.f, yaw_var, H_YAW);
-
-	return yaw_var;
+	const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance());
+	const auto rot_var_ned = matrix::SquareMatrix<float, State::quat_nominal.dof>(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
+	return rot_var_ned(2);
 }
 
 #if defined(CONFIG_EKF2_BAROMETER)
@@ -899,7 +888,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	const Quatf quat_before_reset = _state.quat_nominal;
 
 	// save a copy of covariance in NED frame to restore it after the quat reset
-	const matrix::SquareMatrix3f rot_cov = diag(calcRotVecVariances());
+	const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance());
 	Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
 
 	// update the yaw angle variance

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -62,7 +62,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 
 	// Observation jacobian and Kalman gain vectors
 	VectorState H;
-	const VectorState state_vector = _state.vector();
+	const auto state_vector = _state.vector();
 	sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
 
 	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -94,7 +94,7 @@ void Ekf::fuseOptFlow()
 	// a positive offset in earth frame leads to a smaller height above the ground.
 	float range = predictFlowRange();
 
-	const VectorState state_vector = _state.vector();
+	const auto state_vector = _state.vector();
 
 	Vector2f innov_var;
 	VectorState H;