From b0ad8269a5f49d8fb1624e15845a052a60cae69d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 Nov 2017 11:00:16 +1100 Subject: [PATCH] EKF: enable separate monitoring of aux velocity innovations --- EKF/control.cpp | 8 ++++---- EKF/ekf.h | 7 ++++++- EKF/ekf_helper.cpp | 6 ++++++ EKF/estimator_interface.h | 3 +++ EKF/vel_pos_fusion.cpp | 29 +++++++++++++++++++---------- 5 files changed, 38 insertions(+), 15 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f8cfa69344..bade57c099 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1368,10 +1368,10 @@ void Ekf::controlAuxVelFusion() bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow; if (data_ready && primary_aiding) { - _fuse_vert_vel = _fuse_pos = _fuse_height = false; - _fuse_hor_vel = true; - _vel_pos_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0); - _vel_pos_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1); + _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; + _fuse_hor_vel_aux = true; + _aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0); + _aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1); _velObsVarNE = _auxvel_sample_delayed.velVarNE; _hvelInnovGate = _params.auxvel_gate; fuseVelPosHeight(); diff --git a/EKF/ekf.h b/EKF/ekf.h index ad9637967f..6eb944c790 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -60,6 +60,9 @@ public: // 0-2 vel, 3-5 pos void get_vel_pos_innov(float vel_pos_innov[6]); + // gets the innovations for of the NE auxiliary velocity measurement + void get_aux_vel_innov(float aux_vel_innov[2]); + // gets the innovations of the earth magnetic field measurements void get_mag_innov(float mag_innov[3]); @@ -260,6 +263,7 @@ private: bool _fuse_pos{false}; ///< true when gps position data should be fused bool _fuse_hor_vel{false}; ///< true when gps horizontal velocity measurement should be fused bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused + bool _fuse_hor_vel_aux{false}; ///< true when auxiliary horizontal velocity measurement should be fused float _posObsNoiseNE; ///< 1-STD observtion noise used for the fusion of NE position data (m) float _posInnovGateNE; ///< Number of standard deviations used for the NE position fusion innovation consistency check @@ -319,8 +323,9 @@ private: float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix - float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m**2) + float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) float _vel_pos_innov_var[6] {}; ///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) + float _aux_vel_innov[2] {}; ///< NE auxiliary velocity innovations: (m/sec) float _mag_innov[3] {}; ///< earth magnetic field innovations (Gauss) float _mag_innov_var[3] {}; ///< earth magnetic field innovation variance (Gauss**2) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 70ba3294f0..33f4c274a6 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -756,6 +756,12 @@ void Ekf::get_vel_pos_innov(float vel_pos_innov[6]) memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6); } +// gets the innovations of the earth magnetic field measurements +void Ekf::get_aux_vel_innov(float aux_vel_innov[2]) +{ + memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2); +} + // writes the innovations of the earth magnetic field measurements void Ekf::get_mag_innov(float mag_innov[3]) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 580ef102fd..59872cbd84 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -61,6 +61,9 @@ public: // 0-2 vel, 3-5 pos virtual void get_vel_pos_innov(float vel_pos_innov[6]) = 0; + // gets the innovations for of the NE auxiliary velocity measurement + virtual void get_aux_vel_innov(float aux_vel_innov[2]) = 0; + // gets the innovations of the earth magnetic field measurements virtual void get_mag_innov(float mag_innov[3]) = 0; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 83c13a2a2a..65b7f1a8e4 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -51,22 +51,31 @@ void Ekf::fuseVelPosHeight() 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 + float innovation[6]; // local copy of innovations for [VN,VE,VD,PN,PE,PD] + memcpy(innovation, _vel_pos_innov, sizeof(_vel_pos_innov)); // calculate innovations, innovations gate sizes and observation variances - if (_fuse_hor_vel) { + if (_fuse_hor_vel || _fuse_hor_vel_aux) { // enable fusion for NE velocity axes fuse_map[0] = fuse_map[1] = true; + // handle special case where we are getting velocity observations from an auxiliary source + if (!_fuse_hor_vel) { + innovation[0] = _aux_vel_innov[0]; + innovation[1] = _aux_vel_innov[1]; + } + // Set observation noise variance and innovation consistency check gate size for the NE position observations R[0] = _velObsVarNE(0); R[1] = _velObsVarNE(1); gate_size[1] = gate_size[0] = _hvelInnovGate; + } if (_fuse_vert_vel) { fuse_map[2] = true; // vertical velocity innovation - _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); + innovation[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 @@ -90,7 +99,7 @@ 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) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; + innovation[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]; @@ -114,7 +123,7 @@ void Ekf::fuseVelPosHeight() } 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; + innovation[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); @@ -127,7 +136,7 @@ void Ekf::fuseVelPosHeight() } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) { fuse_map[5] = true; // use range finder with tilt correction - _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, + innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); @@ -136,7 +145,7 @@ void Ekf::fuseVelPosHeight() } else if (_control_status.flags.ev_hgt) { 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); + innovation[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]; @@ -153,7 +162,7 @@ void Ekf::fuseVelPosHeight() unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index]; // Compute the ratio of innovation to gate size - _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * + _vel_pos_test_ratio[obs_index] = sq(innovation[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov_var[obs_index]); } } @@ -170,13 +179,13 @@ void Ekf::fuseVelPosHeight() innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; // record the successful velocity fusion event - if (vel_check_pass && _fuse_hor_vel) { + if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) { _time_last_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_vel_NED = false; } else if (!vel_check_pass) { _innov_check_fail_status.flags.reject_vel_NED = true; } - _fuse_hor_vel = false; + _fuse_hor_vel = _fuse_hor_vel_aux = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) { @@ -278,7 +287,7 @@ void Ekf::fuseVelPosHeight() fixCovarianceErrors(); // apply the state corrections - fuse(Kfusion, _vel_pos_innov[obs_index]); + fuse(Kfusion, innovation[obs_index]); } } }