fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 10:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Protect against divergence

Browse Source
This commit is contained in:
Lorenz Meier
2014-03-18 09:21:27 +01:00
parent 03ccee289b
commit 7cdb7291af
1 changed files with 2 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/estimator.cpp
+2
View File
@@ -312,6 +312,8 @@ void UpdateStrapdownEquationsNED()
states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
// Constrain states (to protect against filter divergence)
ConstrainStates();
} }
void CovariancePrediction(float dt) void CovariancePrediction(float dt)