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

EKF: Add GPS height option and improve height recovery

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-16 10:40:51 +11:00
parent 63b0cf4360
commit 6b2e2dba90
10 changed files with 261 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+25 -11
View File
@@ -60,7 +60,7 @@ void Ekf::fuseVelPosHeight()
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
R[0] = fmaxf(R[0], _gps_speed_accuracy);
R[0] = fmaxf(R[0], _gps_sample_delayed.sacc);
R[0] = R[0] * R[0];
R[1] = R[0];
// innovation gate sizes
@@ -75,7 +75,7 @@ void Ekf::fuseVelPosHeight()
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy);
R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc);
R[2] = R[2] * R[2];
// innovation gate size
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
@@ -95,7 +95,7 @@ void Ekf::fuseVelPosHeight()
} else {
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_hpos_accuracy, lower_limit, upper_limit);
R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
@@ -110,13 +110,26 @@ void Ekf::fuseVelPosHeight()
if (_control_status.flags.baro_hgt) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt + _baro_hgt_offset);
_vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
} else if (_control_status.flags.gps_hgt) {
fuse_map[5] = true;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
// observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
} else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) {
fuse_map[5] = true;
// use range finder with tilt correction
@@ -128,9 +141,10 @@ void Ekf::fuseVelPosHeight()
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
}
}
// calculate innovation test ratios
// calculate innovation test ratios
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (fuse_map[obs_index]) {
// compute the innovation variance SK = HPH + R
@@ -142,9 +156,9 @@ void Ekf::fuseVelPosHeight()
}
}
// check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors
// always pass position checks if using synthetic position measurements
// check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors
// always pass position checks if using synthetic position measurements
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
&& (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
@@ -154,19 +168,19 @@ void Ekf::fuseVelPosHeight()
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
// record the successful velocity fusion time
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
_tilt_err_vec.setZero();
}
// record the successful position fusion time
// record the successful position fusion time
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
_tilt_err_vec.setZero();
}
// record the successful height fusion time
// record the successful height fusion time
if (innov_check_pass_map[5] && _fuse_height) {
_time_last_hgt_fuse = _time_last_imu;
}