From 7de1d39ce431971389fcfc3193570d642fe7f3a2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 3 Feb 2016 18:15:38 +1100 Subject: [PATCH] EKF: Improve use of position and velocity observation noise parameters Fix bug where noise parameters were not being squared when calculating the observation variance. Use GPS reported speed accuracy to set velocity observation noise and use the parameter to set the minimum. --- EKF/estimator_interface.cpp | 1 + EKF/estimator_interface.h | 3 ++- EKF/vel_pos_fusion.cpp | 37 ++++++++++++++++++++++++++++--------- 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 1c891cf337..19ecad30e6 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -137,6 +137,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data)); _gps_speed_valid = gps->vel_ned_valid; + _gps_speed_accuracy = gps->sacc; float lpos_x = 0.0f; float lpos_y = 0.0f; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index dd6252175b..f38721f1dc 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -197,7 +197,8 @@ protected: bool _NED_origin_initialised = false; bool _gps_speed_valid = false; - struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) + float _gps_speed_accuracy = 0.0f; // GPS receiver reported speed accuracy (m/s) + struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) bool _mag_healthy = false; // computed by mag innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 2d0a2ac241..ac378fe730 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -43,44 +43,63 @@ void Ekf::fuseVelPosHeight() { - bool fuse_map[6] = {}; - bool innov_check_pass_map[6] = {}; - float R[6] = {}; - float gate_size[6] = {}; - float Kfusion[24] = {}; + bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available + bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations + float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD] + float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations + float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used - // calculate innovations and gate sizes + // calculate innovations, innovations gate sizes and observation variances if (_fuse_hor_vel) { fuse_map[0] = fuse_map[1] = true; + // horizontal velocity innovations _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); _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] = R[0] * R[0]; R[1] = R[0]; + // innovation gate sizes gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f); gate_size[1] = gate_size[0]; } if (_fuse_vert_vel) { fuse_map[2] = true; - _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); - R[2] = 1.5f * fmaxf(_params.gps_vel_noise, 0.01f); + // vertical velocity innovation + _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); + // 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] = R[2] * R[2]; + // innovation gate size gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f); } 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 R[3] = fmaxf(_params.gps_pos_noise, 0.01f); + R[3] = R[3] * R[3]; R[4] = R[3]; + // innovation gate sizes gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f); gate_size[4] = gate_size[3]; } if (_fuse_height) { fuse_map[5] = true; - _vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis + // vertical position innovation - baro measurement has opposite sign to earth z axis + _vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); + // 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); }