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

ekf2 height reset: do not apply height change to height state twice (bug)

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-18 07:58:19 +02:00
committed by Lorenz Meier
parent 1093e6d789
commit 17c195d1cd
1 changed files with 9 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+9 -14
View File
@@ -271,9 +271,15 @@ void Ekf::resetHeight()
_state_reset_status.velD_counter++;
}
// add the reset amount to the output observer states
_output_new.pos(2) += _state_reset_status.posD_change;
_output_new.vel(2) += _state_reset_status.velD_change;
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
if (vert_pos_reset) {
_output_new.pos(2) += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
_output_new.vel(2) += _state_reset_status.velD_change;
}
// add the reset amount to the output observer buffered data
outputSample output_states;
@@ -292,17 +298,6 @@ void Ekf::resetHeight()
_output_buffer.push_to_index(i,output_states);
}
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
if (vert_pos_reset) {
_output_new.pos(2) += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
_output_new.vel(2) += _state_reset_status.velD_change;
}
}
// align output filter states to match EKF states at the fusion time horizon