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

EKF: make output predictor states consistent with position reset

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-23 17:19:28 +10:00
parent 54d90261d5
commit 79705da7e6
1 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
EKF/ekf_helper.cpp
View File

@ -90,6 +90,11 @@ bool Ekf::resetVelocity()
// gps measurement then use for position initialisation
bool Ekf::resetPosition()
{
// used to calculate the position change due to the reset
Vector2f posNE_before_reset;
posNE_before_reset(0) = _state.pos(0);
posNE_before_reset(1) = _state.pos(1);
if (_control_status.flags.gps) {
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
gpsSample gps_newest = _gps_buffer.get_newest();
@ -134,6 +139,20 @@ bool Ekf::resetPosition()
} else {
return false;
}
// calculate the change in position and apply to the output predictor state history
Vector3f posNE_change;
posNE_change(0) = _state.pos(0) - posNE_before_reset(0);
posNE_change(1) = _state.pos(1) - posNE_before_reset(1);
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.pos(0) += posNE_change(0);
output_states.pos(1) += posNE_change(1);
_output_buffer.push_to_index(index,output_states);
}
}
// Reset height state using the last height measurement