From ccbcbbe26832146a7e0d9d14b82bfd0e19b93691 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 24 May 2024 16:21:44 +0200 Subject: [PATCH] wind_est_replay: report scale instead of inverse_scale The estimator internally estimates the scale inverse, but the interface should be the scale as "airspeed_corrected = scale * airspeed" --- src/lib/wind_estimator/python/wind_estimator_replay.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/wind_estimator/python/wind_estimator_replay.py b/src/lib/wind_estimator/python/wind_estimator_replay.py index 0a165dfe8d..dcc292286d 100644 --- a/src/lib/wind_estimator/python/wind_estimator_replay.py +++ b/src/lib/wind_estimator/python/wind_estimator_replay.py @@ -78,7 +78,9 @@ def run(logfile, use_gnss, scale_init): if scale_init is None: scale_init = 1.0 - state = np.array([0.0, 0.0, scale_init]) + # The estimator estimates the inverse scale factor to have a simpler measurement jacobian + inverse_scale_init = 1 / scale_init + state = np.array([0.0, 0.0, inverse_scale_init]) P = np.diag([1.0, 1.0, 1e-4]) wind_nsd = 1e-2 scale_nsd = 1e-4 @@ -118,7 +120,7 @@ def run(logfile, use_gnss, scale_init): wind_est_n[i] = state[0] wind_est_e[i] = state[1] - scale_est[i] = state[2] + scale_est[i] = 1 / state[2] plt.figure(1) ax1 = plt.subplot(2, 1, 1)