Use new added matrix functions

EKF/EKFGSF_yaw.cpp

@@ -534,9 +534,7 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
 		if (rowLengthSq > FLT_EPSILON) {
 			// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
 			const float rowLengthInv = 1.5f - 0.5f * rowLengthSq;
-			ret(r,0) *= rowLengthInv;
-			ret(r,1) *= rowLengthInv;
-			ret(r,2) *= rowLengthInv;
+			ret.row(r) *=  rowLengthInv;
 		}
         }
 

EKF/covariance.cpp

@@ -953,11 +953,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
 		// calculate accel bias term aligned with the gravity vector
 		const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
-		float down_dvel_bias = 0.0f;
-
-		for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
-			down_dvel_bias += _state.delta_vel_bias(axis_index) * _R_to_earth(2, axis_index);
-		}
+		const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
 
 		// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
 		bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim

EKF/ekf.cpp

@@ -324,9 +324,7 @@ void Ekf::calculateOutputStates()
 	const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr);
 
 	// calculate a yaw change about the earth frame vertical
-	const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
-				     _R_to_earth_now(2, 1) * delta_angle(1) +
-				     _R_to_earth_now(2, 2) * delta_angle(2);
+	const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
 	_yaw_delta_ef += spin_del_ang_D;
 
 	// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic