diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 4980bc24bcae..bc93a702cc99 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -1005,6 +1005,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status"' @@ -1031,6 +1032,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status"' diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 0eaebc42556b..1b430613e90a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -782,6 +782,56 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) _local_position_pub.publish(lpos); } +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(); +} + void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) { // generate vehicle odometry data @@ -813,36 +863,19 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) odom.pitchspeed = rates(1) - gyro_bias(1); odom.yawspeed = rates(2) - gyro_bias(2); - // get the covariance matrix size - static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); - static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); - - // Get covariances to vehicle odometry - float covariances[24]; - _ekf.covariances_diagonal().copyTo(covariances); - - // 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; - } - - // 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]; + // Row-major representation of 6x6 pose cross-covariance matrix URT. + // Order: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis + matrix::SquareMatrix pose_cov{matrix::nans<6, 6>()}; + pose_cov.slice<3, 3>(0, 0) = _ekf.covariances().slice<3, 3>(7, 7); + pose_cov.slice<3, 3>(3, 3) = propagate_covariances_from_quat_to_euler(_ekf.getQuaternion(), + _ekf.covariances().slice<4, 4>(0, 0)); + pose_cov.upper_right_triangle().copyTo(odom.pose_covariance); + + // Row-major representation of 6x6 velocity cross-covariance matrix URT. + // Order: vx, vy, vz, rotation rate about X axis, rotation rate about Y axis, rotation rate about Z axis + matrix::SquareMatrix vel_cov{matrix::nans<6, 6>()}; + vel_cov.slice<3, 3>(0, 0) = _ekf.covariances().slice<3, 3>(4, 4); + vel_cov.upper_right_triangle().copyTo(odom.velocity_covariance); // publish vehicle odometry data odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index bbf6a4905950..881061dcefdd 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -154,6 +154,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem void UpdateMagCalibration(const hrt_abstime ×tamp); + matrix::SquareMatrix propagate_covariances_from_quat_to_euler(const matrix::Quatf &q, + const matrix::SquareMatrix &quat_covariances) const; + /* * Calculate filtered WGS84 height from estimated AMSL height */