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

EKF: Don't use GPS to set position noise when not using GPS

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-16 20:12:36 +11:00
parent c23d72ba29
commit 064a0e4dbc
1 changed files with 14 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+14 -10
View File
@@ -89,8 +89,13 @@ void Ekf::fuseVelPosHeight()
// observation variance - user parameter defined
// if we are in flight and not using GPS, then use a specific parameter
if (!_control_status.flags.gps && _control_status.flags.in_air) {
R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
if (!_control_status.flags.gps) {
if (_control_status.flags.in_air) {
R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
R[3] = _params.gps_pos_noise;
}
} else {
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
@@ -144,7 +149,7 @@ void Ekf::fuseVelPosHeight()
}
// 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
@@ -156,9 +161,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;
@@ -168,19 +173,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;
}
@@ -258,5 +263,4 @@ void Ekf::fuseVelPosHeight()
makeSymmetrical();
limitCov();
}
}