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

EKF: make output predictor states consistent with velocity reset

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-23 17:13:19 +10:00
parent 52229da089
commit 54d90261d5
1 changed files with 15 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+15
View File
@@ -51,6 +51,10 @@
// gps measurement then use for velocity initialisation
bool Ekf::resetVelocity()
{
// used to calculate the velocity change due to the reset
Vector3f vel_before_reset = _state.vel;
// reset EKF states
if (_control_status.flags.gps) {
// if we have a valid GPS measurement use it to initialise velocity states
gpsSample gps_newest = _gps_buffer.get_newest();
@@ -69,6 +73,17 @@ bool Ekf::resetVelocity()
} else {
return false;
}
// calculate the change in velocity and apply to the output predictor state history
Vector3f velocity_change = _state.vel - vel_before_reset;
outputSample output_states;
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index=0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
output_states.vel += velocity_change;
_output_buffer.push_to_index(index,output_states);
}
}
// Reset position states. If we have a recent and valid