From fd72578e982098ec35d005512216224acfb3a968 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 24 Jul 2024 12:32:23 -0400
Subject: [PATCH] ekf2: avoid constraining parameters every iteration

---
 src/modules/ekf2/EKF/covariance.cpp | 15 +++++++--------
 src/modules/ekf2/EKF/ekf.cpp        | 17 +++++++++++++++++
 2 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp
index 520b7855443c..787d1636e2ab 100644
--- a/src/modules/ekf2/EKF/covariance.cpp
+++ b/src/modules/ekf2/EKF/covariance.cpp
@@ -115,11 +115,11 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
 
 	// gyro noise variance
-	float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
+	float gyro_noise = _params.gyro_noise;
 	const float gyro_var = sq(gyro_noise);
 
 	// accel noise variance
-	float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
+	float accel_noise = _params.accel_noise;
 	Vector3f accel_var;
 
 	for (unsigned i = 0; i < 3; i++) {
@@ -143,7 +143,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// gyro bias: add process noise
 	{
-		const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
+		const float gyro_bias_sig = dt * _params.gyro_bias_p_noise;
 		const float gyro_bias_process_noise = sq(gyro_bias_sig);
 
 		for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
@@ -157,7 +157,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// accel bias: add process noise
 	{
-		const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
+		const float accel_bias_sig = dt * _params.accel_bias_p_noise;
 		const float accel_bias_process_noise = sq(accel_bias_sig);
 
 		for (unsigned index = 0; index < State::accel_bias.dof; index++) {
@@ -172,7 +172,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// mag_I: add process noise
-	float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
+	float mag_I_sig = dt * _params.mage_p_noise;
 	float mag_I_process_noise = sq(mag_I_sig);
 
 	for (unsigned index = 0; index < State::mag_I.dof; index++) {
@@ -184,7 +184,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 
 	// mag_B: add process noise
-	float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
+	float mag_B_sig = dt * _params.magb_p_noise;
 	float mag_B_process_noise = sq(mag_B_sig);
 
 	for (unsigned index = 0; index < State::mag_B.dof; index++) {
@@ -201,8 +201,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 #if defined(CONFIG_EKF2_WIND)
 
 	// wind vel: add process noise
-	float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f)
-				    * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
+	float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
 	float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
 
 	for (unsigned index = 0; index < State::wind_vel.dof; index++) {
diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index 5efb2b84ff4b..ac75a367d662 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -132,6 +132,8 @@ void Ekf::reset()
 	}
 
 	_zero_velocity_update.reset();
+
+	updateParameters();
 }
 
 bool Ekf::update()
@@ -339,6 +341,21 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
 
 void Ekf::updateParameters()
 {
+	_params.gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
+	_params.accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
+
+	_params.gyro_bias_p_noise = math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
+	_params.accel_bias_p_noise = math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	_params.mage_p_noise = math::constrain(_params.mage_p_noise, 0.f, 1.f);
+	_params.magb_p_noise = math::constrain(_params.magb_p_noise, 0.f, 1.f);
+#endif // CONFIG_EKF2_MAGNETOMETER
+
+#if defined(CONFIG_EKF2_WIND)
+	_params.wind_vel_nsd = math::constrain(_params.wind_vel_nsd, 0.f, 1.f);
+#endif // CONFIG_EKF2_WIND
+
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 	_aux_global_position.updateParameters();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION