fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_att_pos_estimator/estimator.cpp
View File

@ -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)