Protect against divergence

2026-06-27 05:50:35 +08:00 · 2014-03-18 09:21:27 +01:00
parent 03ccee289b
commit 7cdb7291af
1 changed files with 2 additions and 0 deletions
@@ -312,6 +312,8 @@ void  UpdateStrapdownEquationsNED()
    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;

+    // Constrain states (to protect against filter divergence)
+    ConstrainStates();
 }

 void CovariancePrediction(float dt)