From b73086a6e1ca65ea3fe7e54c3b249c1bdbed3464 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Sat, 25 Feb 2023 14:02:59 +0100 Subject: [PATCH] fix state update equation (#3003) --- sw/airborne/modules/ins/vf_extended_float.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/modules/ins/vf_extended_float.c b/sw/airborne/modules/ins/vf_extended_float.c index 2d42b5efd8..04a88aca37 100644 --- a/sw/airborne/modules/ins/vf_extended_float.c +++ b/sw/airborne/modules/ins/vf_extended_float.c @@ -182,7 +182,7 @@ void vff_propagate(float accel, float dt) { /* update state */ vff.zdotdot = accel + 9.81f - vff.bias; - vff.z += (vff.zdot + vff.zdotdot * dt) * dt / 2.f; // trapizium integration + vff.z += (vff.zdot + vff.zdotdot * dt / 2.f) * dt; vff.zdot += vff.zdotdot * dt; /* update covariance */