From 8e30c2666dc77b31664e9907a28ca11f0410f6f6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Nov 2017 07:44:28 +1100 Subject: [PATCH 1/3] EKF: Add support auxiliary velocity observation This enables the EKF to use an additional NE velocity measurement. This can be used to improve position hold stability when landing using a beacon system for positioning by fusing the beacon velocity estimates. --- EKF/common.h | 10 ++++++++++ EKF/control.cpp | 26 ++++++++++++++++++++++++- EKF/ekf.h | 6 ++++++ EKF/estimator_interface.cpp | 38 +++++++++++++++++++++++++++++++++++-- EKF/estimator_interface.h | 7 +++++++ EKF/vel_pos_fusion.cpp | 17 ++++++----------- 6 files changed, 90 insertions(+), 14 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 33c6adf21b..e65b89c452 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -156,6 +156,12 @@ struct dragSample { uint64_t time_us; ///< timestamp of the measurement (uSec) }; +struct auxVelSample { + Vector2f velNE; ///< measured NE velocity relative to the local origin (m/sec) + Vector2f velVarNE; ///< estimated error variance of the NE velocity (m/sec)**2 + uint64_t time_us; ///< timestamp of the measurement (uSec) +}; + // Integer definitions for vdist_sensor_type #define VDIST_SENSOR_BARO 0 ///< Use baro height #define VDIST_SENSOR_GPS 1 ///< Use GPS height @@ -210,6 +216,7 @@ struct parameters { float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float ev_delay_ms{100.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) + float auxvel_delay_ms{0.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec) // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) @@ -326,6 +333,9 @@ struct parameters { float vert_innov_test_lim{4.5f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec) + // auxilliary velocity fusion + float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s) + float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) }; struct stateSample { diff --git a/EKF/control.cpp b/EKF/control.cpp index 55fb0a8a55..ec70953bb4 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -130,9 +130,12 @@ void Ekf::controlFusionModes() // in a single function using sensor data from multiple sources (GPS, baro, range finder, etc) controlVelPosFusion(); - // Additional data from an external vision sensor can also be fused. + // Additional data from an external vision pose estimator can be fused. controlExternalVisionFusion(); + // Additional NE velocity data from an auxiliary sensor can be fused + controlAuxVelFusion(); + // report dead reckoning if we are no longer fusing measurements that directly constrain velocity drift _is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max) && (_time_last_imu - _time_last_delpos_fuse > _params.no_aid_timeout_max) @@ -522,13 +525,17 @@ void Ekf::controlGpsFusion() float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); _posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); + _velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); // calculate 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); _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); // set innovation gate size _posInnovGateNE = fmaxf(_params.posNE_innov_gate, 1.0f); + _hvelInnovGate = fmaxf(_params.vel_innov_gate, 1.0f); } } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) { @@ -1355,3 +1362,20 @@ void Ekf::controlVelPosFusion() } } + +void Ekf::controlAuxVelFusion() +{ + bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed); + 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); + _velObsVarNE = _auxvel_sample_delayed.velVarNE; + _hvelInnovGate = _params.auxvel_gate; + fuseVelPosHeight(); + _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; + } +} diff --git a/EKF/ekf.h b/EKF/ekf.h index 272f85e29e..ad9637967f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -264,6 +264,9 @@ private: 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 + Vector2f _velObsVarNE; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2 + float _hvelInnovGate; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check + // variables used when position data is being fused using a relative position odometry model bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m) @@ -561,6 +564,9 @@ private: // control fusion of velocity and position observations void controlVelPosFusion(); + // control fusion of auxiliary velocity observations + void controlAuxVelFusion(); + // control for height sensor timeouts, sensor changes and state resets void controlHeightSensorTimeouts(); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 9f143c6627..049ea86a16 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -442,6 +442,38 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message } } +void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]) +{ + if (!_initialised || _auxvel_buffer_fail) { + return; + } + + // Allocate the required buffer size if not previously done + // Do not retry if allocation has failed previously + if (_auxvel_buffer.get_length() < _obs_buffer_length) { + _auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length); + if (_auxvel_buffer_fail) { + ECL_ERR("EKF aux vel buffer allocation failed"); + return; + } + } + + // limit data rate to prevent data being lost + if (time_usec - _time_last_auxvel > _min_obs_interval_us) { + + auxVelSample auxvel_sample_new; + auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000; + + auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; + _time_last_auxvel = time_usec; + + auxvel_sample_new.velNE = Vector2f(data); + auxvel_sample_new.velVarNE = Vector2f(variance); + + _auxvel_buffer.push(auxvel_sample_new); + } +} + bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle @@ -450,8 +482,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) math::max(_params.gps_delay_ms, math::max(_params.flow_delay_ms, math::max(_params.ev_delay_ms, - math::max(_params.min_delay_ms, - math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))))); + math::max(_params.auxvel_delay_ms, + math::max(_params.min_delay_ms, + math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))))); // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; @@ -511,6 +544,7 @@ void EstimatorInterface::unallocate_buffers() _output_buffer.unallocate(); _output_vert_buffer.unallocate(); _drag_buffer.unallocate(); + _auxvel_buffer.unallocate(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 4312895b84..580ef102fd 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -185,6 +185,9 @@ public: // set external vision position and attitude data void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); + // set auxiliary velocity data + void setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]); + // return a address to the parameters struct // in order to give access to the application parameters *getParamHandle() {return &_params;} @@ -402,6 +405,7 @@ protected: extVisionSample _ev_sample_delayed{}; dragSample _drag_sample_delayed{}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) + auxVelSample _auxvel_sample_delayed{}; // Used by the multi-rotor specific drag force fusion uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate @@ -463,6 +467,7 @@ protected: RingBuffer _output_buffer; RingBuffer _output_vert_buffer; RingBuffer _drag_buffer; + RingBuffer _auxvel_buffer; // observation buffer final allocation failed bool _gps_buffer_fail{false}; @@ -473,6 +478,7 @@ protected: bool _flow_buffer_fail{false}; bool _ev_buffer_fail{false}; bool _drag_buffer_fail{false}; + bool _auxvel_buffer_fail{false}; uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds uint64_t _time_last_gps{0}; // timestamp of last gps measurement in microseconds @@ -483,6 +489,7 @@ protected: uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds uint64_t _time_last_optflow{0}; uint64_t _time_last_gnd_effect_on{0}; //last time the baro ground effect compensation was turned on externally (uSec) + uint64_t _time_last_auxvel{0}; fault_status_u _fault_status{}; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 8db642591b..c1b2ccff94 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -54,18 +54,13 @@ void Ekf::fuseVelPosHeight() // calculate innovations, innovations gate sizes and observation variances if (_fuse_hor_vel) { + // enable fusion for NE velocity axes 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_sample_delayed.sacc); - 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]; + + // 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) { From 2a57fd858d2c5b7083806a26031d4988b828866a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 Nov 2017 09:52:34 +1100 Subject: [PATCH 2/3] EKF: clean up reset of fusion flags --- EKF/control.cpp | 2 -- EKF/vel_pos_fusion.cpp | 3 +++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index ec70953bb4..f8cfa69344 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1358,7 +1358,6 @@ void Ekf::controlVelPosFusion() // Fuse available NED velocity and position data into the main filter if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; } } @@ -1376,6 +1375,5 @@ void Ekf::controlAuxVelFusion() _velObsVarNE = _auxvel_sample_delayed.velVarNE; _hvelInnovGate = _params.auxvel_gate; fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; } } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c1b2ccff94..83c13a2a2a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -176,6 +176,7 @@ void Ekf::fuseVelPosHeight() } else if (!vel_check_pass) { _innov_check_fail_status.flags.reject_vel_NED = true; } + _fuse_hor_vel = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) { @@ -188,6 +189,7 @@ void Ekf::fuseVelPosHeight() } else if (!pos_check_pass) { _innov_check_fail_status.flags.reject_pos_NE = true; } + _fuse_pos = false; // record the successful height fusion event if (innov_check_pass_map[5] && _fuse_height) { @@ -196,6 +198,7 @@ void Ekf::fuseVelPosHeight() } else if (!innov_check_pass_map[5]) { _innov_check_fail_status.flags.reject_pos_D = true; } + _fuse_height = false; for (unsigned obs_index = 0; obs_index < 6; obs_index++) { // skip fusion if not requested or checks have failed From b0ad8269a5f49d8fb1624e15845a052a60cae69d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 Nov 2017 11:00:16 +1100 Subject: [PATCH 3/3] 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]); } } }