From f1b82057c09aab4aaa2f80050ec0be230b9cbcc7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Jan 2016 22:46:30 +1100 Subject: [PATCH] EKF: Combine GPS velocity innovation gate parameters Separate vertical and horizontal parameters for GPS velocity innovation gates are not required --- EKF/estimator_base.h | 7 +++---- EKF/vel_pos_fusion.cpp | 4 ++-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 30f4424ccd..18194140b2 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -134,10 +134,9 @@ namespace estimator { float gps_vel_noise = 0.05f; float gps_pos_noise = 1.0f; float baro_noise = 0.1f; - float baro_innov_gate = 5.0f; // barometer fusion innovation consistency gate size in standard deviations - float velD_innov_gate = 5.0f; // Vertical velocity fusion innovation consistency gate size in standard deviations - float posNE_innov_gate = 5.0f; // Horizontal position innovation consistency gate size in standard deviations - float velNE_innov_gate = 5.0f; // Horizontal velocity fusion innovation consistency gate size in standard deviations + float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations + float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations + float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion float mag_declination_deg = 0.0f; // magnetic declination in degrees diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 1ca95847bd..5bbc889fff 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -56,7 +56,7 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1); R[0] = _params.gps_vel_noise; R[1] = _params.gps_vel_noise; - gate_size[0] = fmaxf(_params.velNE_innov_gate, 1.0f); + gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f); gate_size[1] = gate_size[0]; } @@ -64,7 +64,7 @@ void Ekf::fuseVelPosHeight() fuse_map[2] = true; _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); R[2] = _params.gps_vel_noise; - gate_size[2] = fmaxf(_params.velD_innov_gate, 1.0f); + gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f); } if (_fuse_pos) {