dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

proper handling of position and velocity reset 

- position reset method was returning before the actual delta values
were written and applied to the output buffer
- apply reset delta also to the output sample which was already taken out
from the output buffer, otherwise the complementary filter solution is
offset from the ekf solution

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-10-11 13:59:04 +02:00 committed by Lorenz Meier
parent 79b1d3018d
commit 238d947239
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EKF/ekf_helper.cpp
View File

@ -73,6 +73,10 @@ bool Ekf::resetVelocity()
} }
// apply the change in velocity to our newest velocity estimate
// which was already taken out from the output buffer
_output_new.vel += velocity_change;
// capture the reset event // capture the reset event
_state_reset_status.velNE_change(0) = velocity_change(0); _state_reset_status.velNE_change(0) = velocity_change(0);
_state_reset_status.velNE_change(1) = velocity_change(1); _state_reset_status.velNE_change(1) = velocity_change(1);
@ -96,18 +100,15 @@ bool Ekf::resetPosition()
// this reset is only called if we have new gps data at the fusion time horizon // this reset is only called if we have new gps data at the fusion time horizon
_state.pos(0) = _gps_sample_delayed.pos(0); _state.pos(0) = _gps_sample_delayed.pos(0);
_state.pos(1) = _gps_sample_delayed.pos(1); _state.pos(1) = _gps_sample_delayed.pos(1);
return true;
} else if (_control_status.flags.opt_flow) { } else if (_control_status.flags.opt_flow) {
_state.pos(0) = 0.0f; _state.pos(0) = 0.0f;
_state.pos(1) = 0.0f; _state.pos(1) = 0.0f;
return true;
} else if (_control_status.flags.ev_pos) { } else if (_control_status.flags.ev_pos) {
// this reset is only called if we have new ev data at the fusion time horizon // this reset is only called if we have new ev data at the fusion time horizon
_state.pos(0) = _ev_sample_delayed.posNED(0); _state.pos(0) = _ev_sample_delayed.posNED(0);
_state.pos(1) = _ev_sample_delayed.posNED(1); _state.pos(1) = _ev_sample_delayed.posNED(1);
return true;
} else { } else {
return false; return false;
@ -126,6 +127,11 @@ bool Ekf::resetPosition()
_output_buffer.push_to_index(index,output_states); _output_buffer.push_to_index(index,output_states);
} }
// apply the change in position to our newest position estimate
// which was already taken out from the output buffer
_output_new.pos(0) += posNE_change(0);
_output_new.pos(1) += posNE_change(1);
// capture the reset event // capture the reset event
_state_reset_status.posNE_change = posNE_change; _state_reset_status.posNE_change = posNE_change;
_state_reset_status.posNE_counter++; _state_reset_status.posNE_counter++;