EKF: use Matrix cross product

PX4 · Dec 17, 2019 · 98a1aae · 98a1aae
1 parent 0831c15
commit 98a1aae
Show file tree

Hide file tree

Showing 7 changed files with 7 additions and 22 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -571,9 +571,6 @@ class EstimatorInterface
 	// 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
@@ -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
@@ -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;