From 050c9dcd3d93382ea6448229458e81bdbeed79e0 Mon Sep 17 00:00:00 2001
From: Mathieu Bresciani <brescianimathieu@gmail.com>
Date: Tue, 27 Oct 2020 16:56:32 +0100
Subject: [PATCH] HTE: fix variance prediction (#16016)

In the predition step, the process variance was erroneously
multiplied by dt instead of dt^2. The default values are adjusted
accordingly to keep the same tuning for the default loop rate of 50Hz
---
 .../mc_hover_thrust_estimator/hover_thrust_estimator_params.c | 2 +-
 .../mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp | 4 ++--
 .../mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c b/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c
index 597d46f4a6..47d7f6ea38 100644
--- a/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c
+++ b/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c
@@ -52,7 +52,7 @@
  * @unit normalized_thrust/s
  * @group Hover Thrust Estimator
  */
-PARAM_DEFINE_FLOAT(HTE_HT_NOISE, 0.0005);
+PARAM_DEFINE_FLOAT(HTE_HT_NOISE, 0.0036);
 
 /**
  * Gate size for acceleration fusion
diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
index 53ed68e5a9..435789fb86 100644
--- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
+++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
@@ -45,7 +45,7 @@ void ZeroOrderHoverThrustEkf::predict(const float dt)
 {
 	// State is constant
 	// Predict state covariance only
-	_state_var += _process_var * dt;
+	_state_var += _process_var * dt * dt;
 	_dt = dt;
 }
 
@@ -133,7 +133,7 @@ inline bool ZeroOrderHoverThrustEkf::isLargeOffsetDetected() const
 
 inline void ZeroOrderHoverThrustEkf::bumpStateVariance()
 {
-	_state_var += 1e3f * _process_var * _dt;
+	_state_var += 1e3f * _process_var * _dt * _dt;
 }
 
 inline void ZeroOrderHoverThrustEkf::updateLpf(const float residual, const float signed_innov_test_ratio)
diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
index cfbbdefd53..b7e7efb33e 100644
--- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
+++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
@@ -84,7 +84,7 @@ public:
 
 	void resetAccelNoise() { _acc_var = 5.f; };
 
-	void predict(float _dt);
+	void predict(float dt);
 	void fuseAccZ(float acc_z, float thrust, status &status_return);
 
 	void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
@@ -102,7 +102,7 @@ private:
 
 	float _gate_size{3.f};
 	float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2)
-	float _process_var{0.25e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2)
+	float _process_var{12.5e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2)
 	float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
 	float _dt{0.02f};