From 440465702e3a6fc6afc93f2d424cb1a4e6ab838d Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 16 May 2024 17:21:50 +0200 Subject: [PATCH] wind_est_replay: fix cov matrix format and data indexing --- src/lib/wind_estimator/python/wind_estimator_replay.py | 4 ++-- 1 file changed, 2 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 efa19eaf0e..a44d62db4f 100644 --- a/src/lib/wind_estimator/python/wind_estimator_replay.py +++ b/src/lib/wind_estimator/python/wind_estimator_replay.py @@ -103,12 +103,12 @@ def run(logfile, use_gnss): P += Q * dt - if t_true_airspeed[i_airspeed] < t_now: + if i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now: while i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now: i_airspeed += 1 i_airspeed -= 1 - (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P.flatten(), true_airspeed[i_airspeed], R, epsilon) + (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P, true_airspeed[i_airspeed], R, epsilon) state += np.array(K) * innov P -= K * H * P i_airspeed += 1