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) {