diff --git a/src/lib/ecl b/src/lib/ecl index c4492b17c1d7..3f691891c48f 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit c4492b17c1d7bb9b74011efa9890452b091421e8 +Subproject commit 3f691891c48f4e87ae038a485a5f0de39039f0f7 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 47ea7b44a2a2..94d93d991ed2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -165,6 +165,17 @@ class Ekf2 final : public ModuleBase, 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 propagate_covariances_from_quat_to_euler( + const matrix::Quatf &q, + const matrix::SquareMatrix &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, 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 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 pose_cov = matrix::eye(); + 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 vel_cov = matrix::diag(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 Ekf2::propagate_covariances_from_quat_to_euler( + const matrix::Quatf &q, + const matrix::SquareMatrix &quat_covariances) const +{ + // Jacobian matrix (3x4) containing the partial derivatives of the + // Euler angle equations with respect to the quaternions + matrix::Matrix 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();