Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ekf2: fill pose and vel covariance in odometry #16929

Closed
wants to merge 1 commit into from
Closed

ekf2: fill pose and vel covariance in odometry #16929

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .ci/Jenkinsfile-hardware
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1016,6 +1016,7 @@ void statusFTDI() {
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_event_flags"'
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"'
Expand All @@ -1042,6 +1043,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"'
Expand Down
93 changes: 63 additions & 30 deletions src/modules/ekf2/EKF2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -843,6 +843,56 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
_local_position_pub.publish(lpos);
}

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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where did these come from?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will see if this derivation can be added to our existing sympy script.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See https://github.com/priseborough/PX4-Autopilot/tree/pr-ekf2_derivation_update for a symbolic derivation

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nuno's original PR for context. #11092

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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This doesn't protect against /0 where we divide by d1. Likewise for d2.

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 &timestamp, const imuSample &imu)
{
// generate vehicle odometry data
Expand Down Expand Up @@ -874,36 +924,19 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, 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<float, 6> 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<float, 6> 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();
Expand Down
3 changes: 3 additions & 0 deletions src/modules/ekf2/EKF2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem

void UpdateMagCalibration(const hrt_abstime &timestamp);

matrix::SquareMatrix<float, 3> propagate_covariances_from_quat_to_euler(const matrix::Quatf &q,
const matrix::SquareMatrix<float, 4> &quat_covariances) const;

/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
Expand Down