Improve Matrix library usage

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 ""

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) {

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;

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

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);

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;

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;
 }

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 {