EKF2: add usage to new exposed covariance

src/modules/ekf2/ekf2_main.cpp

@@ -165,6 +165,17 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 	 */
 	float filter_altitude_ellipsoid(float amsl_hgt);
 
+	/* @brief Covariance propagation from quaternions to euler angles using the covariance propagation law
+	 * Source: "Development of a Real-Time Attitude System Using a Quaternion
+	 * Parameterization and Non-Dedicated GPS Receivers", John B. Schleppe 1996
+	 * @param  q			quaternion of the orientation in the local NED frame
+	 * @param  quat_covariances	orientation quaternion covariances
+	 * @return orientation euler angle covariances
+	 */
+	matrix::SquareMatrix<float, 3> propagate_covariances_from_quat_to_euler(
+		const matrix::Quatf &q,
+		const matrix::SquareMatrix<float, 4> &quat_covariances) const;
+
 	bool 	_replay_mode = false;			///< true when we use replay data from a log
 
 	// time slip monitoring
@@ -523,7 +534,6 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 		_param_ekf2_move_test	///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
 
 	)
-
 };
 
 Ekf2::Ekf2():
@@ -1317,6 +1327,9 @@ void Ekf2::run()
 			filter_control_status_u control_status;
 			_ekf.get_control_mode(&control_status.value);
 
+			// get the full covariance matrix
+			matrix::SquareMatrix<float, 24> covariances = _ekf.covariances();
+
 			// only publish position after successful alignment
 			if (control_status.flags.tilt_align) {
 				// generate vehicle local position data
@@ -1455,36 +1468,18 @@ void Ekf2::run()
 					lpos.hagl_max = INFINITY;
 				}
 
-				// Get covariances to vehicle odometry
-				float covariances[24];
-				_ekf.covariances_diagonal().copyTo(covariances);
-
-				// get the covariance matrix size
-				const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
-				const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
-
-				// initially set pose covariances to 0
-				for (size_t i = 0; i < POS_URT_SIZE; i++) {
-					odom.pose_covariance[i] = 0.0;
-				}
-
-				// set the position variances
-				odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
-				odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
-				odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
-
-				// TODO: implement propagation from quaternion covariance to Euler angle covariance
-				// by employing the covariance law
-
-				// initially set velocity covariances to 0
-				for (size_t i = 0; i < VEL_URT_SIZE; i++) {
-					odom.velocity_covariance[i] = 0.0;
-				}
+				// get the position and orientation covariances and fill the pose covariance matrix
+				// it's not a cross-covariance matrix but simplifies propagating the data
+				matrix::SquareMatrix<float, 6> pose_cov = matrix::eye<float, 6>();
+				pose_cov.set(covariances.slice<3, 3>(7, 7), 0, 0);
+				pose_cov.set(propagate_covariances_from_quat_to_euler(q, covariances.slice<4, 4>(0, 0)), 3, 3);
+				pose_cov.upper_right_triangle().copyTo(odom.pose_covariance);
 
-				// set the linear velocity variances
-				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
-				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
-				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
+				// get the velocity covariances
+				// note: unknown angular velocity covariance matrix - diag set to NaN
+				matrix::SquareMatrix<float, 6> vel_cov = matrix::diag<float, 6>(matrix::nans<6, 1>());
+				vel_cov.set(covariances.slice<3, 3>(4, 4), 0, 0);
+				vel_cov.upper_right_triangle().copyTo(odom.velocity_covariance);
 
 				// publish vehicle local position data
 				_vehicle_local_position_pub.update();
@@ -1574,7 +1569,7 @@ void Ekf2::run()
 			status.timestamp = now;
 			_ekf.get_state_delayed(status.states);
 			status.n_states = 24;
-			_ekf.covariances_diagonal().copyTo(status.covariances);
+			covariances.diag().copyTo(status.covariances);
 			_ekf.get_gps_check_status(&status.gps_check_fail_flags);
 			// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 			// the GPS Fix bit, which is always checked)
@@ -2481,6 +2476,57 @@ float Ekf2::filter_altitude_ellipsoid(float amsl_hgt)
 	return amsl_hgt + _wgs84_hgt_offset;
 }
 
+matrix::SquareMatrix<float, 3> Ekf2::propagate_covariances_from_quat_to_euler(
+	const matrix::Quatf &q,
+	const matrix::SquareMatrix<float, 4> &quat_covariances) const
+{
+	// Jacobian matrix (3x4) containing the partial derivatives of the
+	// Euler angle equations with respect to the quaternions
+	matrix::Matrix<float, 3, 4> G;
+
+	// quaternion components
+	float q1 = q(0);
+	float q2 = q(1);
+	float q3 = q(2);
+	float q4 = q(3);
+
+	// numerator components
+	float n1 =  2 * q1 * q2 + 2 * q2 * q4;
+	float n2 = -2 * q2 * q2 - 2 * q3 * q3 + 1;
+	float n3 =  2 * q1 * q4 + 2 * q2 * q3;
+	float n4 = -2 * q3 * q3 - 2 * q4 * q4 + 1;
+	float n5 =  2 * q1 * q3 + 2 * q2 * q4;
+	float n6 = -2 * q1 * q2 - 2 * q2 * q4;
+	float n7 = -2 * q1 * q4 - 2 * q2 * q3;
+
+	// Protect against division by 0
+	float d1 = n1 * n1 + n2 * n2;
+	float d2 = n3 * n3 + n4 * n4;
+	d1 = math::max(0.0f, d1);
+	d2 = math::max(0.0f, d2);
+
+	// Protect against square root of negative numbers
+	float x = math::max(-n5 * n5 + 1, 0.0f);
+
+	// compute G matrix
+	float sqrt_x = sqrtf(x);
+	float g00_03 = 2 * q2 * n2 / d1;
+	G(0, 0) =  g00_03;
+	G(0, 1) = -4 * q2 * n6 / d1 + (2 * q1 + 2 * q4) * n2 / d1;
+	G(0, 2) = -4 * q3 * n6 / d1;
+	G(0, 3) =  g00_03;
+	G(1, 0) =  2 * q3 / sqrt_x;
+	G(1, 1) =  2 * q4 / sqrt_x;
+	G(1, 2) =  2 * q1 / sqrt_x;
+	G(1, 3) =  2 * q2 / sqrt_x;
+	G(2, 0) =  2 * q4 * n4 / d2;
+	G(2, 1) =  2 * q3 * n4 / d2;
+	G(2, 2) =  2 * q2 * n4 / d2 - 4 * q3 * n7 / d2;
+	G(2, 3) =  2 * q1 * n4 / d2 - 4 * q4 * n7 / d2;
+
+	return G * quat_covariances * G.transpose();
+}
+
 Ekf2 *Ekf2::instantiate(int argc, char *argv[])
 {
 	Ekf2 *instance = new Ekf2();