diff --git a/EKF/control.cpp b/EKF/control.cpp index 401abc79b2..79112911db 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -208,10 +208,10 @@ void Ekf::controlExternalVisionFusion() } if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW) - && !_ev_rot_mat_initialised) { + && !_R_ev_to_ekf_initialised) { // Reset transformation between EV and EKF navigation frames when starting fusion resetExtVisRotMat(); - _ev_rot_mat_initialised = true; + _R_ev_to_ekf_initialised = true; ECL_INFO_TIMESTAMPED("external vision aligned"); } } @@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion() Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev; // rotate measurement into body frame is required when fusing with GPS - ev_delta_pos = _ev_rot_mat * ev_delta_pos; + ev_delta_pos = _R_ev_to_ekf * ev_delta_pos; // use the change in position since the last measurement _ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); @@ -319,7 +319,7 @@ void Ekf::controlExternalVisionFusion() // use the absolute position Vector3f ev_pos_meas = _ev_sample_delayed.pos; if (_params.fusion_mode & MASK_ROTATE_EV) { - ev_pos_meas = _ev_rot_mat * ev_pos_meas; + ev_pos_meas = _R_ev_to_ekf * ev_pos_meas; } _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0); _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1); @@ -353,7 +353,7 @@ void Ekf::controlExternalVisionFusion() // rotate measurement into correct earth frame if required if (_params.fusion_mode & MASK_ROTATE_EV) { - vel_aligned = _ev_rot_mat * _ev_sample_delayed.vel; + vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel; } // correct velocity for offset relative to IMU @@ -807,7 +807,7 @@ void Ekf::controlHeightSensorTimeouts() // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data const bool reset_to_gps = !_gps_hgt_intermittent && - ( (gps_hgt_accurate && !prev_bad_vert_accel) || !baro_data_available ); + ((gps_hgt_accurate && !prev_bad_vert_accel) || !baro_data_available); if (reset_to_gps) { // set height sensor health @@ -844,8 +844,8 @@ void Ekf::controlHeightSensorTimeouts() // if baro data is acceptable and GPS data is inaccurate, reset height to baro const bool reset_to_baro = baro_data_fresh && - ( (baro_data_consistent && !_baro_hgt_faulty && !gps_hgt_accurate) || - _gps_hgt_intermittent ); + ((baro_data_consistent && !_baro_hgt_faulty && !gps_hgt_accurate) || + _gps_hgt_intermittent); if (reset_to_baro) { // set height sensor health diff --git a/EKF/ekf.h b/EKF/ekf.h index 5f3f434419..4e2964e700 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -322,9 +322,9 @@ private: 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 AxisAnglef _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad) - Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity + Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec) - bool _ev_rot_mat_initialised{0}; ///< _ev_rot_mat should only be initialised once in the beginning through the reset function + bool _R_ev_to_ekf_initialised{0}; ///< _R_ev_to_ekf should only be initialised once in the beginning through the reset function // 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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 3fd8a3b84e..3a320b1d2d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -97,7 +97,7 @@ bool Ekf::resetVelocity() ECL_INFO_TIMESTAMPED("reset velocity to ev velocity"); Vector3f _ev_vel = _ev_sample_delayed.vel; if(_params.fusion_mode & MASK_ROTATE_EV){ - _ev_vel = _ev_rot_mat *_ev_sample_delayed.vel; + _ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel; } _state.vel = _ev_vel; P.uncorrelateCovarianceSetVariance<3>(4, sq(_ev_sample_delayed.velErr)); @@ -159,7 +159,7 @@ bool Ekf::resetPosition() // this reset is only called if we have new ev data at the fusion time horizon Vector3f _ev_pos = _ev_sample_delayed.pos; if(_params.fusion_mode & MASK_ROTATE_EV){ - _ev_pos = _ev_rot_mat *_ev_sample_delayed.pos; + _ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos; } _state.pos(0) = _ev_pos(0); _state.pos(1) = _ev_pos(1); @@ -403,10 +403,10 @@ bool Ekf::realignYawGPS() const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); // calculate course yaw angle - const float ekfGOG = atan2f(_state.vel(1), _state.vel(0)); + const float ekfCOG = atan2f(_state.vel(1), _state.vel(0)); // Check the EKF and GPS course over ground for consistency - const float courseYawError = gpsCOG - ekfGOG; + const float courseYawError = gpsCOG - ekfCOG; // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad const bool badYawErr = fabsf(courseYawError) > 0.5f; @@ -629,7 +629,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // calculate the observed yaw angle if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // calculate the yaw angle for a 312 sequence euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); @@ -686,7 +686,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool _flt_mag_align_start_time = _imu_sample_delayed.time_us; // calculate the amount that the quaternion has changed by - const Quatf q_error( (quat_after_reset * quat_before_reset.inversed()).normalized() ); + const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized()); // update quaternion states _state.quat_nominal = quat_after_reset; @@ -790,9 +790,9 @@ void Ekf::constrainStates() // calculate the earth rotation vector Vector3f Ekf::calcEarthRateNED(float lat_rad) const { - return Vector3f{CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad), + return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad), 0.0f, - -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)}; + -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) @@ -1634,7 +1634,7 @@ void Ekf::startMag3DFusion() void Ekf::calcExtVisRotMat() { // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon. - const Quatf q_error( (_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized() ); + const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized()); // convert to a delta angle and apply a spike and low pass filter AxisAnglef rot_vec(q_error); @@ -1657,7 +1657,7 @@ void Ekf::calcExtVisRotMat() } - _ev_rot_mat = Dcmf(_ev_rot_vec_filt); // rotation from EV reference to EKF reference + _R_ev_to_ekf = Dcmf(_ev_rot_vec_filt); } @@ -1680,7 +1680,7 @@ void Ekf::resetExtVisRotMat() _ev_rot_vec_filt.zero(); } - _ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference + _R_ev_to_ekf = Dcmf(q_error); } // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame