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

EKF: Add ability to use EV and GPS data together

Browse Source 
Fuse external vision data using a relative position odometry method when GPS data is also being used and enable both GPOS and EV data to be fused on the same time step.
This commit is contained in:
Paul Riseborough
2017-07-26 18:06:18 +10:00
parent 74d1955dfa
commit e08da1c599
4 changed files with 79 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+27 -3
View File
@@ -99,10 +99,34 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.ev_pos) {
// we are using external vision measurements
// calculate innovations
if(_hpos_odometry) {
if(!_hpos_prev_available) {
// no previous observation available to calculate position change
fuse_map[3] = fuse_map[4] = false;
_hpos_prev_available = true;
} else {
// use the change in position since the last measurement
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0);
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1);
}
// record observation and estimate for use next time
_hpos_meas_prev(0) = _ev_sample_delayed.posNED(0);
_hpos_meas_prev(1) = _ev_sample_delayed.posNED(1);
_hpos_pred_prev(0) = _state.pos(0);
_hpos_pred_prev(1) = _state.pos(1);
} else {
// use the absolute position
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
}
// observation 1-STD error
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
// innovation gate size
gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);