diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-18 09:21:27 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-18 09:21:27 +0100 |
| commit | 7cdb7291af52f2b60aa53607e7ec229fecf7497f (patch) | |
| tree | 9dfb2a93dfb17ce16d688389e513bdbddbfe2d1e /src/modules/fw_att_pos_estimator/estimator.cpp | |
| parent | 03ccee289ba6484a889ed6f79fd744b412bc8537 (diff) | |
| download | px4-firmware-7cdb7291af52f2b60aa53607e7ec229fecf7497f.tar.gz px4-firmware-7cdb7291af52f2b60aa53607e7ec229fecf7497f.tar.bz2 px4-firmware-7cdb7291af52f2b60aa53607e7ec229fecf7497f.zip | |
Protect against divergence
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) |