diff options
Diffstat (limited to 'src/modules/fw_att_pos_estimator/estimator.cpp')
| -rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 2 |
1 files changed, 2 insertions, 0 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e21b94c43..fa81d4dfa 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) |