Protect against divergence

2014-03-18 09:21:27 +01:00 · 2014-03-18 09:21:27 +01:00 · 7cdb7291af
parent 03ccee289b
commit 7cdb7291af
1 changed files with 2 additions and 0 deletions
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@ -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)