From 48787c0160512e1ad66bafb4a800dab6b470451b Mon Sep 17 00:00:00 2001 From: kamilritz Date: Wed, 25 Dec 2019 17:21:15 +0100 Subject: [PATCH] Improve Matrix library usage --- CMakeLists.txt | 2 +- EKF/control.cpp | 9 +++------ EKF/ekf.cpp | 14 ++++---------- EKF/ekf.h | 2 +- EKF/ekf_helper.cpp | 18 ++++++------------ EKF/gps_yaw_fusion.cpp | 6 ++---- EKF/mag_fusion.cpp | 2 +- EKF/optflow_fusion.cpp | 7 +------ 8 files changed, 19 insertions(+), 41 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e9211c9ebf..9ffed1e7fa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,7 +132,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) include(ExternalProject) ExternalProject_Add(matrix GIT_REPOSITORY "https://github.com/PX4/Matrix.git" - GIT_TAG 4f3565da94d00c4cd1feb560f1f72a81296522f8 + GIT_TAG e81483a808ce7b0217c11d3dc0fce90685f44353 UPDATE_COMMAND "" PATCH_COMMAND "" CONFIGURE_COMMAND "" diff --git a/EKF/control.cpp b/EKF/control.cpp index 3854af1128..b8c0686a75 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -223,8 +223,7 @@ void Ekf::controlExternalVisionFusion() if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observation quaternion // get the roll, pitch, yaw estimates from the quaternion states - Quatf q_init(_state.quat_nominal); - Eulerf euler_init(q_init); + Eulerf euler_init(_state.quat_nominal); // get initial yaw from the observation quaternion const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); @@ -350,7 +349,7 @@ void Ekf::controlExternalVisionFusion() Vector3f ev_vel_obs_var; Vector2f ev_vel_innov_gates; - Vector3f vel_aligned{_ev_sample_delayed.vel}; + Vector3f vel_aligned(_ev_sample_delayed.vel); // rotate measurement into correct earth frame if required if (_params.fusion_mode & MASK_ROTATE_EV) { @@ -364,9 +363,7 @@ void Ekf::controlExternalVisionFusion() const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; vel_aligned -= vel_offset_earth; - _ev_vel_innov(0) = _state.vel(0) - vel_aligned(0); - _ev_vel_innov(1) = _state.vel(1) - vel_aligned(1); - _ev_vel_innov(2) = _state.vel(2) - vel_aligned(2); + _ev_vel_innov = _state.vel - vel_aligned; // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max) { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8adf386180..52647624c1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -255,9 +255,7 @@ void Ekf::predictState() // correct delta angles for earth rotation rate corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; - // convert the delta angle to a delta quaternion - Quatf dq; - dq.from_axis_angle(corrected_delta_ang); + const Quatf dq(AxisAnglef{corrected_delta_ang}); // rotate the previous quaternion by the delta quaternion using a quaternion multiplication _state.quat_nominal = _state.quat_nominal * dq; @@ -337,7 +335,7 @@ bool Ekf::collect_imu(const imuSample &imu) _imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt); imuSample imu_sample_new; - imu_sample_new.delta_ang = _q_down_sampled.to_axis_angle(); + imu_sample_new.delta_ang = AxisAnglef(_q_down_sampled); imu_sample_new.delta_vel = _imu_down_sampled.delta_vel; imu_sample_new.delta_ang_dt = _imu_down_sampled.delta_ang_dt; imu_sample_new.delta_vel_dt = _imu_down_sampled.delta_vel_dt; @@ -400,9 +398,7 @@ void Ekf::calculateOutputStates() // Note fixed coefficients are used to save operations. The exact time constant is not important. _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt; - // convert the delta angle to an equivalent delta quaternions - Quatf dq; - dq.from_axis_angle(delta_angle); + const Quatf dq(AxisAnglef{delta_angle}); // rotate the previous INS quaternion by the delta quaternions _output_new.time_us = imu.time_us; @@ -468,9 +464,7 @@ void Ekf::calculateOutputStates() _output_vert_delayed = _output_vert_buffer.get_oldest(); // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon - Quatf quat_inv = _state.quat_nominal.inversed(); - Quatf q_error = quat_inv * _output_sample_delayed.quat_nominal; - q_error.normalize(); + const Quatf q_error( (_state.quat_nominal.inversed() * _output_sample_delayed.quat_nominal).normalized() ); // convert the quaternion delta to a delta angle float scalar; diff --git a/EKF/ekf.h b/EKF/ekf.h index d9f8156201..5f3f434419 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -321,7 +321,7 @@ private: 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 EV to EKF reference, initiliazied to zero rotation (rad) + 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 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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 76c8708f24..513469d169 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -686,8 +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 - Quatf q_error = quat_after_reset * quat_before_reset.inversed(); - q_error.normalize(); + const Quatf q_error( (quat_after_reset * quat_before_reset.inversed()).normalized() ); // update quaternion states _state.quat_nominal = quat_after_reset; @@ -1637,11 +1636,10 @@ 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. - Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed(); - q_error.normalize(); + 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 - Vector3f rot_vec = q_error.to_axis_angle(); + AxisAnglef rot_vec(q_error); float rot_vec_norm = rot_vec.norm(); @@ -1663,9 +1661,7 @@ void Ekf::calcExtVisRotMat() } - // convert filtered vector to a quaternion and then to a rotation matrix - q_error.from_axis_angle(_ev_rot_vec_filt); - _ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference + _ev_rot_mat = Dcmf(_ev_rot_vec_filt); // rotation from EV reference to EKF reference } @@ -1677,8 +1673,7 @@ void Ekf::resetExtVisRotMat() Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed(); q_error.normalize(); - // convert to a delta angle and reset - Vector3f rot_vec = q_error.to_axis_angle(); + AxisAnglef rot_vec(q_error); float rot_vec_norm = rot_vec.norm(); @@ -1696,8 +1691,7 @@ void Ekf::resetExtVisRotMat() // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame void Ekf::get_ev2ekf_quaternion(float *quat) { - Quatf quat_ev2ekf; - quat_ev2ekf.from_axis_angle(_ev_rot_vec_filt); + const Quatf quat_ev2ekf(_ev_rot_vec_filt); for (unsigned i = 0; i < 4; i++) { quat[i] = quat_ev2ekf(i); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 984cc42800..17ec1e02de 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -319,8 +319,7 @@ bool Ekf::resetGpsAntYaw() // to avoid gimbal lock if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { // get the roll, pitch, yaw estimates from the quaternion states using a 321 Tait-Bryan rotation sequence - Quatf q_init(_state.quat_nominal); - Eulerf euler_init(q_init); + Eulerf euler_init(_state.quat_nominal); // correct the yaw angle euler_init(2) += yaw_delta; @@ -366,8 +365,7 @@ bool Ekf::resetGpsAntYaw() } // calculate the amount that the quaternion has changed by - Quatf q_error = _state.quat_nominal * quat_before_reset.inversed(); - q_error.normalize(); + const Quatf q_error( (_state.quat_nominal * quat_before_reset.inversed()).normalized() ); // convert the quaternion delta to a delta angle Vector3f delta_ang_error; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index dffbae595a..c6041793c4 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -1004,7 +1004,7 @@ float Ekf::calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f)); // calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component - float mag_z_body_pred = _R_to_earth(0,2) * mag_earth_predicted(0) + _R_to_earth(1,2) * mag_earth_predicted(1) + _R_to_earth(2,2) * mag_earth_predicted(2); + const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.slice<3,1>(0,2)); return mag_z_body_pred < 0 ? -mag_z_abs : mag_z_abs; } diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index a84129f697..6e936f080f 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -535,12 +535,7 @@ bool Ekf::calcOptFlowBodyRateComp() const Vector3f measured_body_rate(_flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt)); // calculate the bias estimate using a combined LPF and spike filter - _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), - -0.1f, 0.1f); - _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), - -0.1f, 0.1f); - _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), - -0.1f, 0.1f); + _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f; } } else {