fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 02:53:34 +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-06-01 17:13:00 +10:00
parent 54d90261d5
commit 79705da7e6
1 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+19
View File
@@ -90,6 +90,11 @@ bool Ekf::resetVelocity()
// gps measurement then use for position initialisation // gps measurement then use for position initialisation
bool Ekf::resetPosition() 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 (_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 // 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(); gpsSample gps_newest = _gps_buffer.get_newest();
@@ -134,6 +139,20 @@ bool Ekf::resetPosition()
} else { } else {
return false; 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 // Reset height state using the last height measurement