ekf2: access state covariance using helper functions

src/modules/ekf2/EKF/covariance.cpp

@@ -541,15 +541,10 @@ void Ekf::resetQuatCov(const float yaw_noise)
 
 void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
 {
-	// clear existing quaternion covariance
-	// Optimization: avoid the creation of a <4> function
-	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f);
-	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx + 2, 0.0f);
-
 	matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
 	sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
 	q_cov.copyLowerToUpperTriangle();
-	P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx) = q_cov;
+	resetStateCovariance<State::quat_nominal>(q_cov);
 }
 
 void Ekf::resetMagCov()
@@ -565,8 +560,8 @@ void Ekf::resetMagCov()
 
 	saveMagCovData();
 #else
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
+	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);
 
 #endif
 }

src/modules/ekf2/EKF/ekf.h

@@ -308,9 +308,13 @@ public:
 
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
+	Vector2f getWindVelocityVariance() const { return getStateVariance<State::wind_vel>(); }
 
-	// get the wind velocity var
+	template <const IdxDof &S>
-	Vector2f getWindVelocityVariance() const { return P.slice<State::wind_vel.dof, State::wind_vel.dof>(State::wind_vel.idx, State::wind_vel.idx).diag(); }
+	matrix::Vector<float, S.dof>getStateVariance() const { return P.slice<S.dof, S.dof>(S.idx, S.idx).diag(); } // calling getStateCovariance().diag() uses more flash space
+
+	template <const IdxDof &S>
+	matrix::SquareMatrix<float, S.dof>getStateCovariance() const { return P.slice<S.dof, S.dof>(S.idx, S.idx); }
 
 	// get the full covariance matrix
 	const matrix::SquareMatrix<float, State::size> &covariances() const { return P; }
@@ -318,14 +322,10 @@ public:
 	// get the diagonal elements of the covariance matrix
 	matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
 
-	// get the orientation (quaterion) covariances
+	matrix::Vector<float, State::quat_nominal.dof> getQuaternionVariance() const { return getStateVariance<State::quat_nominal>(); }
-	matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx); }
+	Vector3f getVelocityVariance() const { return getStateVariance<State::vel>(); };
+	Vector3f getPositionVariance() const { return getStateVariance<State::pos>(); }
 
-	// get the linear velocity covariances
-	matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<State::vel.dof, State::vel.dof>(State::vel.idx, State::vel.idx); }
-
-	// get the position covariances
-	matrix::SquareMatrix<float, 3> position_covariances() const { return P.slice<State::pos.dof, State::pos.dof>(State::pos.idx, State::pos.idx); }
 
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	bool collect_gps(const gpsMessage &gps) override;
@@ -356,10 +356,6 @@ public:
 	void resetGyroBias();
 	void resetAccelBias();
 
-	Vector3f getVelocityVariance() const { return velocity_covariances().diag(); };
-
-	Vector3f getPositionVariance() const { return position_covariances().diag(); }
-
 	// First argument returns GPS drift  metrics in the following array locations
 	// 0 : Horizontal position drift rate (m/s)
 	// 1 : Vertical position drift rate (m/s)
@@ -408,23 +404,22 @@ public:
 
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
-	Vector3f getGyroBiasVariance() const { return P.slice<State::gyro_bias.dof, State::gyro_bias.dof>(State::gyro_bias.idx, State::gyro_bias.idx).diag(); } // get the gyroscope bias variance in rad/s
+	Vector3f getGyroBiasVariance() const { return getStateVariance<State::gyro_bias>(); } // get the gyroscope bias variance in rad/s
 	float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
 
 	// accel bias
 	const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
-	Vector3f getAccelBiasVariance() const { return P.slice<State::accel_bias.dof, State::accel_bias.dof>(State::accel_bias.idx, State::accel_bias.idx).diag(); } // get the accelerometer bias variance in m/s**2
+	Vector3f getAccelBiasVariance() const { return getStateVariance<State::accel_bias>(); } // get the accelerometer bias variance in m/s**2
 	float getAccelBiasLimit() const { return _params.acc_bias_lim; }
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	const Vector3f &getMagEarthField() const { return _state.mag_I; }
 
-	// mag bias (states 19, 20, 21)
 	const Vector3f &getMagBias() const { return _state.mag_B; }
 	Vector3f getMagBiasVariance() const
 	{
 		if (_control_status.flags.mag) {
-			return P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx).diag();
+			return getStateVariance<State::mag_B>();
 		}
 
 		return _saved_mag_bf_covmat.diag();
@@ -805,6 +800,12 @@ private:
 	// predict ekf covariance
 	void predictCovariance(const imuSample &imu_delayed);
 
+	template <const IdxDof &S>
+	void resetStateCovariance(const matrix::SquareMatrix<float, S.dof> &cov) {
+		P.uncorrelateCovarianceSetVariance<S.dof>(S.idx, 0.0f);
+		P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
+	}
+
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -625,7 +625,7 @@ void Ekf::resetGyroBias()
 	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
 
 	// Set previous frame values
-	_prev_gyro_bias_var = P.slice<State::gyro_bias.dof, State::gyro_bias.dof>(State::gyro_bias.idx, State::gyro_bias.idx).diag();
+	_prev_gyro_bias_var = getStateVariance<State::gyro_bias>();
 }
 
 void Ekf::resetAccelBias()
@@ -638,7 +638,7 @@ void Ekf::resetAccelBias()
 	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
 
 	// Set previous frame values
-	_prev_accel_bias_var = P.slice<State::accel_bias.dof, State::accel_bias.dof>(State::accel_bias.idx, State::accel_bias.idx).diag();
+	_prev_accel_bias_var = getStateVariance<State::accel_bias>();
 }
 
 // get EKF innovation consistency check status information comprising of:

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -164,7 +164,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 	if (measurement_valid && quality_sufficient) {
 		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
 		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
-		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + P.slice<2, 2>(State::pos.idx, State::pos.idx).diag());
+		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance<State::pos>()));
 	}
 
 	if (!measurement_valid) {

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -167,8 +167,8 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 			    || wmm_updated
 			    || !_mag_decl_cov_reset
 			    || !_state.mag_I.longerThan(0.f)
-			    || (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I
+			    || (getStateVariance<State::mag_I>().min() < sq(0.0001f))
-			    || (P.slice<3, 3>(19, 19).diag().min() < sq(0.0001f)) // mag_B
+			    || (getStateVariance<State::mag_B>().min() < sq(0.0001f))
 			   ) {
 				ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
 
@@ -220,19 +220,17 @@ void Ekf::stopMagFusion()
 void Ekf::saveMagCovData()
 {
 	// save the NED axis covariance sub-matrix
-	_saved_mag_ef_covmat = P.slice<State::mag_I.dof, State::mag_I.dof>(State::mag_I.idx, State::mag_I.idx);
+	_saved_mag_ef_covmat = getStateCovariance<State::mag_I>();
 
 	// save the XYZ body covariance sub-matrix
-	_saved_mag_bf_covmat = P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx);
+	_saved_mag_bf_covmat = getStateCovariance<State::mag_B>();
 }
 
 void Ekf::loadMagCovData()
 {
 	// re-instate the NED axis covariance sub-matrix
-	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
+	resetStateCovariance<State::mag_I>(_saved_mag_ef_covmat);
-	P.slice<State::mag_I.dof, State::mag_I.dof>(State::mag_I.idx, State::mag_I.idx) = _saved_mag_ef_covmat;
 
 	// re-instate the XYZ body axis covariance sub-matrix
-	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);
+	resetStateCovariance<State::mag_B>(_saved_mag_bf_covmat);
-	P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx) = _saved_mag_bf_covmat;
 }

src/modules/ekf2/EKF/zero_velocity_update.cpp

@@ -55,7 +55,7 @@ void Ekf::controlZeroVelocityUpdate()
 			// Set a low variance initially for faster leveling and higher
 			// later to let the states follow the measurements
 			const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
-			Vector3f innov_var = P.slice<3, 3>(State::vel.idx, State::vel.idx).diag() + obs_var;
+			Vector3f innov_var = getVelocityVariance() + obs_var;
 
 			for (unsigned i = 0; i < 3; i++) {
 				fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i);

src/modules/ekf2/EKF2.cpp

@@ -1665,10 +1665,10 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
 	angular_velocity.copyTo(odom.angular_velocity);
 
 	// velocity covariances
-	_ekf.velocity_covariances().diag().copyTo(odom.velocity_variance);
+	_ekf.getVelocityVariance().copyTo(odom.velocity_variance);
 
 	// position covariances
-	_ekf.position_covariances().diag().copyTo(odom.position_variance);
+	_ekf.getPositionVariance().copyTo(odom.position_variance);
 
 	// orientation covariance
 	_ekf.calcRotVecVariances().copyTo(odom.orientation_variance);

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -280,11 +280,6 @@ float EkfWrapper::getYawAngle() const
 	return euler(2);
 }
 
-matrix::Vector4f EkfWrapper::getQuaternionVariance() const
-{
-	return matrix::Vector4f(_ekf->orientation_covariances().diag());
-}
-
 int EkfWrapper::getQuaternionResetCounter() const
 {
 	float tmp[4];

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -118,7 +118,6 @@ public:
 
 	Eulerf getEulerAngles() const;
 	float getYawAngle() const;
-	matrix::Vector4f getQuaternionVariance() const;
 	int getQuaternionResetCounter() const;
 
 	void enableDragFusion();

src/modules/ekf2/test/test_EKF_gps_yaw.cpp

@@ -333,9 +333,9 @@ TEST_F(EkfGpsHeadingTest, stopOnGround)
 	_ekf_wrapper.setMagFuseTypeNone();
 
 	// WHEN: running without yaw aiding
-	const matrix::Vector4f quat_variance_before = _ekf_wrapper.getQuaternionVariance();
+	const matrix::Vector4f quat_variance_before = _ekf->getQuaternionVariance();
 	_sensor_simulator.runSeconds(20.0);
-	const matrix::Vector4f quat_variance_after = _ekf_wrapper.getQuaternionVariance();
+	const matrix::Vector4f quat_variance_after = _ekf->getQuaternionVariance();
 
 	// THEN: the yaw variance increases
 	EXPECT_GT(quat_variance_after(3), quat_variance_before(3));

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -78,7 +78,7 @@ public:
 
 	void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f)
 	{
-		const matrix::Vector4f quat_variance = _ekf_wrapper.getQuaternionVariance();
+		const matrix::Vector4f quat_variance = _ekf->getQuaternionVariance();
 		EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1);
 		EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2);
 		EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3);