diff --git a/EKF/common.h b/EKF/common.h index a998987e1d..8512028626 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -174,6 +174,7 @@ struct dragSample { #define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision NED position data #define MASK_USE_EVYAW (1<<4) ///< set to true to use exernal vision quaternion data for yaw #define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind +#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic diff --git a/EKF/control.cpp b/EKF/control.cpp index 5daa3e92ca..086bc573ac 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -133,6 +133,7 @@ void Ekf::controlFusionModes() // 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) && (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max) && (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max); @@ -143,6 +144,13 @@ void Ekf::controlExternalVisionFusion() // Check for new exernal vision data if (_ev_data_ready) { + // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames + // needs to be calculated and the observations rotated into the EKF frame of reference + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + // rotate EV measurements into the EKF Navigation frame + calcExtVisRotMat(); + } + // external vision position aiding selection logic if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon @@ -154,12 +162,17 @@ void Ekf::controlExternalVisionFusion() // reset the position if we are not already aiding using GPS, else use a relative position // method for fusing the position data if (_control_status.flags.gps) { - _hpos_odometry = true; + _fuse_hpos_as_odom = true; } else { resetPosition(); resetVelocity(); - _hpos_odometry = false; + // we cannot use an absolue position from a rotating reference frame + if (_params.fusion_mode & MASK_ROTATE_EV) { + _fuse_hpos_as_odom = true; + } else { + _fuse_hpos_as_odom = false; + } } } @@ -243,16 +256,57 @@ void Ekf::controlExternalVisionFusion() _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); - // if GPS data is being used, then use an incremental position fusion method for EV data - if (_control_status.flags.gps) { - _hpos_odometry = true; + // Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED + if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) { + _fuse_hpos_as_odom = true; + } else { + _fuse_hpos_as_odom = false; } + + if(_fuse_hpos_as_odom) { + if(!_hpos_prev_available) { + // no previous observation available to calculate position change + _fuse_pos = false; + _hpos_prev_available = true; + + } else { + // calculate the change in position since the last measurement + Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev; + + // rotate measurement into body frame if required + if (_params.fusion_mode & MASK_ROTATE_EV) { + ev_delta_pos = _ev_rot_mat * ev_delta_pos; + } + + // use the change in position since the last measurement + _vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); + _vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); + + } + + // record observation and estimate for use next time + _pos_meas_prev = _ev_sample_delayed.posNED; + _hpos_pred_prev(0) = _state.pos(0); + _hpos_pred_prev(1) = _state.pos(1); + + } else { + // use the absolute position + _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); + _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); + } + + // observation 1-STD error + _posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f); + + // innovation gate size + _posInnovGateNE = fmaxf(_params.ev_innov_gate, 1.0f); } // Fuse available NED position data into the main filter if (_fuse_height || _fuse_pos) { fuseVelPosHeight(); _fuse_pos = _fuse_height = false; + _fuse_hpos_as_odom = false; } @@ -423,12 +477,16 @@ void Ekf::controlGpsFusion() } // handle the case when we now have GPS, but have not been using it for an extended period - if (_control_status.flags.gps && !_control_status.flags.opt_flow) { - // We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something - bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max); + if (_control_status.flags.gps) { + // We are relying on aiding to constrain drift so after a specified time + // with no aiding we need to do something + bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) + && (_time_last_imu - _time_last_delpos_fuse > _params.no_gps_timeout_max) + && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max) + && (_time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max); - // Our position measurments have been rejected for more than 14 seconds - do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max; + // We haven't had an absolute position fix for a longer time so need to do something + do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max); if (do_reset) { // Reset states to the last GPS measurement @@ -469,6 +527,19 @@ void Ekf::controlGpsFusion() _gps_sample_delayed.pos(1) -= pos_offset_earth(1); _gps_sample_delayed.hgt += pos_offset_earth(2); + // calculate observation process noise + 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); + + // calculate innovations + _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); + + } } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) { @@ -1251,6 +1322,7 @@ void Ekf::controlVelPosFusion() _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); + _fuse_hpos_as_odom = false; ECL_WARN("EKF stopping navigation"); } @@ -1258,6 +1330,17 @@ void Ekf::controlVelPosFusion() _fuse_pos = true; _time_last_fake_gps = _time_last_imu; + if (_control_status.flags.in_air && _control_status.flags.tilt_align) { + _posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + } else { + _posObsNoiseNE = 0.5f; + } + _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); + _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); + + // glitch protection is not required so set gate to a large value + _posInnovGateNE = 100.0f; + } // Fuse available NED velocity and position data into the main filter diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index ef49ed9032..f8701da0ab 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -273,6 +273,11 @@ bool Ekf::initialiseFilter() // calculate the initial magnetic field and yaw alignment _control_status.flags.yaw_align = resetMagHeading(mag_init); + // initialise the rotation from EV to EKF navigation frame if required + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + resetExtVisRotMat(); + } + if (_control_status.flags.rng_hgt) { // if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup // so it can be used as a backup ad set the initial height using the range finder @@ -297,6 +302,7 @@ bool Ekf::initialiseFilter() // reset the essential fusion timeout counters _time_last_hgt_fuse = _time_last_imu; _time_last_pos_fuse = _time_last_imu; + _time_last_delpos_fuse = _time_last_imu; _time_last_vel_fuse = _time_last_imu; _time_last_hagl_fuse = _time_last_imu; _time_last_of_fuse = _time_last_imu; diff --git a/EKF/ekf.h b/EKF/ekf.h index 45eaf5b014..2309748ff4 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -226,6 +226,9 @@ public: // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status); + // return the quaternion defining the rotation from the EKF to the External Vision reference frame + void get_ekf2ev_quaternion(float *quat); + private: static constexpr uint8_t _k_num_states{24}; ///< number of EKF states @@ -258,11 +261,17 @@ private: 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 + 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 + // variables used when position data is being fused using a relative position odometry model - bool _hpos_odometry{false}; ///< true when the NE position data is being fused using an odometry assumption - Vector2f _hpos_meas_prev; ///< previous value of NE position measurement fused using odometry assumption (m) + 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) Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m) bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use + Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation from EKF to EV reference (rad) + Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame + uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the ekf to ev rotation matrix was updated (uSec) // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused @@ -276,6 +285,7 @@ private: uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) + uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec) uint64_t _time_last_vel_fuse{0}; ///< time the last fusion of velocity measurements was performed (uSec) uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of height measurements was performed (uSec) uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) @@ -487,6 +497,15 @@ private: // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(); + // update the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame + // and update the rotation matrix which transforms EV navigation frame measurements into NED + void calcExtVisRotMat(); + + + // reset the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame + // and reset the rotation matrix which transforms EV navigation frame measurements into NED + void resetExtVisRotMat(); + // limit the diagonal of the covariance matrix void fixCovarianceErrors(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4c49905671..8a72e70f4a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -607,6 +607,11 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // update transformation matrix from body to world frame using the current estimate _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + // reset the rotation from the EV to EKF frame of reference if it is being used + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + resetExtVisRotMat(); + } + // update the yaw angle variance using the variance of the measurement if (_params.fusion_mode & MASK_USE_EVYAW) { // using error estimate from external vision data @@ -1182,7 +1187,9 @@ bool Ekf::global_position_is_valid() bool Ekf::inertial_dead_reckoning() { bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) - && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max)); + && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) + || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max) + || (_time_last_imu - _time_last_delpos_fuse <= _params.no_aid_timeout_max)); bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max); bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max); @@ -1422,3 +1429,111 @@ void Ekf::setControlEVHeight() _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; } + +// update the estimated misalignment between the EV navigation frame and the EKF navigation frame +// and calculate a rotation matrix which rotates EV measurements into the EKF's navigatin frame +void Ekf::calcExtVisRotMat() +{ + // calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon + Quatf quat_inv = _ev_sample_delayed.quat.inversed(); + Quatf q_error = quat_inv * _state.quat_nominal; + q_error.normalize(); + + // convert to a delta angle and apply a spike and low pass filter + Vector3f rot_vec; + float delta; + float scaler; + if (q_error(0) >= 0.0f) { + delta = 2 * acosf(q_error(0)); + if (delta > 1e-6f) { + scaler = 1.0f / sinf(delta/2); + } else { + // The rotation is too small to calculate a vector accurately + // Make the rotation vector zero + scaler = 0.0f; + } + } else { + delta = 2 * acosf(-q_error(0)); + if (delta > 1e-6f) { + scaler = -1.0f / sinf(delta/2); + } else { + // The rotation is too small to calculate a vector accurately + // Make the rotation vector zero + scaler = 0.0f; + } + } + rot_vec(0) = q_error(2) * scaler; + rot_vec(1) = q_error(3) * scaler; + rot_vec(2) = q_error(4) * scaler; + + float rot_vec_norm = rot_vec.norm(); + if (rot_vec_norm > 1e-6f) { + // ensure magnitude of rotation matches the quaternion + rot_vec = rot_vec * (delta / rot_vec_norm); + + // apply an input limiter to protect from spikes + Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; + float input_delta_mag = _input_delta_vec.norm(); + if (input_delta_mag > 0.1f) { + rot_vec = _ev_rot_vec_filt + rot_vec * (0.1f / input_delta_mag); + } + + // Apply a first order IIR low pass filter + const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec + float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us) , 0.0f , 1.0f); + _ev_rot_last_time_us = _time_last_imu; + _ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha; + + } + + // convert filtered vector to a quaternion and then to a rotation matrix + q_error.from_axis_angle(_ev_rot_vec_filt); + _ev_rot_mat = quat_to_invrotmat(q_error); + +} + +// reset the estimated misalignment between the EV navigation frame and the EKF navigation frame +// and update the rotation matrix which rotates EV measurements into the EKF's navigation frame +void Ekf::resetExtVisRotMat() +{ + // calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon + Quatf quat_inv = _ev_sample_delayed.quat.inversed(); + Quatf q_error = quat_inv * _state.quat_nominal; + q_error.normalize(); + + // convert to a delta angle and reset + Vector3f rot_vec; + float delta; + if (q_error(0) >= 0.0f) { + delta = 2 * acosf(q_error(0)); + rot_vec(0) = q_error(1) / sinf(delta/2); + rot_vec(1) = q_error(2) / sinf(delta/2); + rot_vec(2) = q_error(3) / sinf(delta/2); + } else { + delta = 2 * acosf(-q_error(1)); + rot_vec(0) = -q_error(2) / sinf(delta/2); + rot_vec(1) = -q_error(3) / sinf(delta/2); + rot_vec(2) = -q_error(4) / sinf(delta/2); + } + + float rot_vec_norm = rot_vec.norm(); + if (rot_vec_norm > 1e-9f) { + _ev_rot_vec_filt = rot_vec * delta / rot_vec_norm; + } else { + _ev_rot_vec_filt.zero(); + } + + // reset the rotation matrix + _ev_rot_mat = quat_to_invrotmat(q_error); +} + +// return the quaternions for the rotation from the EKF to the External Vision system frame of reference +void Ekf::get_ekf2ev_quaternion(float *quat) +{ + Quatf quat_ekf2ev; + quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt); + for (unsigned i = 0; i < 4; i++) { + quat[i] = quat_ekf2ev(i); + } +} + diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index bbdb63ca91..40cefeb9d6 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -236,6 +236,9 @@ public: } } + // return the quaternion defining the rotation from the EKF to the External Vision reference frame + virtual void get_ekf2ev_quaternion(float *quat) = 0; + // get the velocity of the body frame origin in local NED earth frame void get_velocity(float *vel) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 01284defa5..d91ca01c3c 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -82,76 +82,12 @@ void Ekf::fuseVelPosHeight() } if (_fuse_pos) { + // enable fusion for the NE position axes fuse_map[3] = fuse_map[4] = true; - // Calculate innovations and observation variance depending on type of observations - // being used - if (_control_status.flags.gps) { - // we are using GPS measurements - float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); - float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); - _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); - _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); - - // innovation gate size - gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f); - - - } else if (_control_status.flags.ev_pos) { - // calculate innovations - if(_hpos_odometry) { - if(!_hpos_prev_available) { - // no previous observation available to calculate position change - fuse_map[3] = fuse_map[4] = false; - _hpos_prev_available = true; - - } else { - // use the change in position since the last measurement - _vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0); - _vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1); - - } - - // record observation and estimate for use next time - _hpos_meas_prev(0) = _ev_sample_delayed.posNED(0); - _hpos_meas_prev(1) = _ev_sample_delayed.posNED(1); - _hpos_pred_prev(0) = _state.pos(0); - _hpos_pred_prev(1) = _state.pos(1); - - } else { - // use the absolute position - _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); - _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); - } - - // observation 1-STD error - R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); - - // innovation gate size - gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f); - - } else { - // No observations - use a static position to constrain drift - if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); - } else { - R[3] = 0.5f; - } - _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); - _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); - - // glitch protection is not required so set gate to a large value - gate_size[3] = 100.0f; - - } - - // convert North position noise to variance - R[3] = R[3] * R[3]; - - // copy North axis values to East axis - R[4] = R[3]; - gate_size[4] = gate_size[3]; + // Set observation noise variance and innovation consistency check gate size for the NE position observations + R[4] = R[3] = sq(_posObsNoiseNE); + gate_size[4] = gate_size[3] = _posInnovGateNE; } @@ -234,7 +170,11 @@ void Ekf::fuseVelPosHeight() // record the successful position fusion event if (pos_check_pass && _fuse_pos) { - _time_last_pos_fuse = _time_last_imu; + if (!_fuse_hpos_as_odom) { + _time_last_pos_fuse = _time_last_imu; + } else { + _time_last_delpos_fuse = _time_last_imu; + } _innov_check_fail_status.flags.reject_pos_NE = false; } else if (!pos_check_pass) { _innov_check_fail_status.flags.reject_pos_NE = true;