From 98a1aae494fed167599811268d09af8c5d61fa03 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Nov 2019 12:05:49 -0500 Subject: [PATCH] EKF: use Matrix cross product --- EKF/control.cpp | 4 ++-- EKF/ekf.cpp | 2 +- EKF/ekf_helper.cpp | 10 ---------- EKF/estimator_interface.cpp | 2 +- EKF/estimator_interface.h | 3 --- EKF/optflow_fusion.cpp | 2 +- EKF/terrain_estimator.cpp | 6 ++---- 7 files changed, 7 insertions(+), 22 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a9e0e2d335..bf80f6020d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -357,7 +357,7 @@ void Ekf::controlExternalVisionFusion() // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body); + Vector3f vel_offset_body = ang_rate % pos_offset_body; Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; vel_aligned -= vel_offset_earth; @@ -692,7 +692,7 @@ void Ekf::controlGpsFusion() // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body); + Vector3f vel_offset_body = ang_rate % pos_offset_body; Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 1ad0ad1443..354eeafac4 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -470,7 +470,7 @@ void Ekf::calculateOutputStates() const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt); // calculate the velocity of the IMU relative to the body origin - const Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body); + const Vector3f vel_imu_rel_body = ang_rate % _params.imu_pos_body; // rotate the relative velocity into earth frame _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index eaf1421526..7ed2ddf63d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1498,16 +1498,6 @@ void Ekf::update_deadreckoning_status() _deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max); } -// perform a vector cross product -Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2) -{ - Vector3f vecOut; - vecOut(0) = vecIn1(1) * vecIn2(2) - vecIn1(2) * vecIn2(1); - vecOut(1) = vecIn1(2) * vecIn2(0) - vecIn1(0) * vecIn2(2); - vecOut(2) = vecIn1(0) * vecIn2(1) - vecIn1(1) * vecIn2(0); - return vecOut; -} - // calculate the inverse rotation matrix from a quaternion rotation // this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ee9e2245be..79d1d18d6f 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -62,7 +62,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) } // calculate a metric which indicates the amount of coning vibration - Vector3f temp = cross_product(imu_sample.delta_ang, _delta_ang_prev); + Vector3f temp = imu_sample.delta_ang % _delta_ang_prev; _vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm(); // calculate a metric which indicates the amount of high frequency gyro vibration diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index f9630de8eb..81caeff32b 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -571,9 +571,6 @@ protected: // this is the previous status of the filter control modes - used to detect mode transitions filter_control_status_u _control_status_prev{}; - // perform a vector cross product - Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2); - // calculate the inverse rotation matrix from a quaternion rotation Matrix3f quat_to_invrotmat(const Quatf &quat); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 706d0622a4..1edba91790 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -74,7 +74,7 @@ void Ekf::fuseOptFlow() // calculate the velocity of the sensor relative to the imu in body frame // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign - Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body); + Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; // calculate the velocity of the sensor in the earth frame Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 2150cb3257..f26e73117d 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -187,9 +187,7 @@ void Ekf::fuseFlowForTerrain() // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed // correct for gyro bias errors in the data used to do the motion compensation // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - Vector2f opt_flow_rate; - opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); - opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); + const Vector2f opt_flow_rate = Vector2f{_flowRadXYcomp} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias}; // get latest estimated orientation float q0 = _state.quat_nominal(0); @@ -208,7 +206,7 @@ void Ekf::fuseFlowForTerrain() // calculate the velocity of the sensor relative to the imu in body frame // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign - Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body); + Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; // calculate the velocity of the sensor in the earth frame Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;