fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

wind_est_replay: fix cov matrix format and data indexing

Browse Source
This commit is contained in:
bresch
2024-05-17 09:17:08 +02:00
committed by Mathieu Bresciani
parent 6a966ab065
commit 440465702e
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/python/wind_estimator_replay.py
+2 -2
View File
@@ -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