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..bade57c099 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)) { @@ -1351,7 +1358,22 @@ 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; } } + +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_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 272f85e29e..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,10 +263,14 @@ 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 + 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) @@ -316,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) @@ -561,6 +569,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/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.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..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; @@ -185,6 +188,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 +408,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 +470,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 +481,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 +492,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..65b7f1a8e4 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -51,27 +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; - // 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]; + + // 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 @@ -95,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]; @@ -119,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); @@ -132,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); @@ -141,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]; @@ -158,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]); } } @@ -175,12 +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 = _fuse_hor_vel_aux = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) { @@ -193,6 +198,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) { @@ -201,6 +207,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 @@ -280,7 +287,7 @@ void Ekf::fuseVelPosHeight() fixCovarianceErrors(); // apply the state corrections - fuse(Kfusion, _vel_pos_innov[obs_index]); + fuse(Kfusion, innovation[obs_index]); } } }