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

EKF: Add fusion of external vision 3D pos data

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-14 21:17:29 +10:00
parent 81469d6621
commit 25f1d1d766
2 changed files with 32 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+23 -7
View File
@@ -83,24 +83,31 @@ void Ekf::fuseVelPosHeight()
if (_fuse_pos) {
fuse_map[3] = fuse_map[4] = true;
// horizontal position innovations
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
// observation variance - user parameter defined
// if we are in flight and not using GPS, then use a specific parameter
if (!_control_status.flags.gps) {
// Calculate innovations and observation variance depending on type of observations
// being used
if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
// No observations - use a static position to constrain drift
if (_control_status.flags.in_air) {
R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
R[3] = _params.gps_pos_noise;
}
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
} else {
} else if (_control_status.flags.gps) {
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
} else if (_control_status.flags.ev_pos) {
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);
}
@@ -145,6 +152,15 @@ void Ekf::fuseVelPosHeight()
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
} else if (_control_status.flags.ev_pos) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
// observation variance - defined externally
R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
}
}