From 7cdb7291af52f2b60aa53607e7ec229fecf7497f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 09:21:27 +0100 Subject: [PATCH] Protect against divergence --- src/modules/fw_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e21b94c43f..fa81d4dfa0 100644 --- 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)