mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 08:17:35 +08:00
msg/vehicle_odometry.msg: simplify covariance handling and update all usage (#19966)
- replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly) - these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit - ekf2: add new helper to get roll/pitch/yaw covariances - mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD) - mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough) Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
+121
-110
@@ -594,7 +594,7 @@ void EKF2::Run()
|
||||
perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
|
||||
|
||||
PublishLocalPosition(now);
|
||||
PublishOdometry(now, imu_sample_new);
|
||||
PublishOdometry(now);
|
||||
PublishGlobalPosition(now);
|
||||
PublishWindEstimate(now);
|
||||
|
||||
@@ -1059,72 +1059,43 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp)
|
||||
_local_position_pub.publish(lpos);
|
||||
}
|
||||
|
||||
void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu)
|
||||
void EKF2::PublishOdometry(const hrt_abstime ×tamp)
|
||||
{
|
||||
// generate vehicle odometry data
|
||||
vehicle_odometry_s odom;
|
||||
odom.timestamp_sample = timestamp;
|
||||
|
||||
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
|
||||
// position
|
||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
||||
_ekf.getPosition().copyTo(odom.position);
|
||||
|
||||
// Vehicle odometry position
|
||||
const Vector3f position{_ekf.getPosition()};
|
||||
odom.x = position(0);
|
||||
odom.y = position(1);
|
||||
odom.z = position(2);
|
||||
|
||||
// Vehicle odometry linear velocity
|
||||
odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
|
||||
const Vector3f velocity{_ekf.getVelocity()};
|
||||
odom.vx = velocity(0);
|
||||
odom.vy = velocity(1);
|
||||
odom.vz = velocity(2);
|
||||
|
||||
// Vehicle odometry quaternion
|
||||
// orientation quaternion
|
||||
_ekf.getQuaternion().copyTo(odom.q);
|
||||
|
||||
// Vehicle odometry angular rates
|
||||
const Vector3f gyro_bias{_ekf.getGyroBias()};
|
||||
const Vector3f rates{imu.delta_ang / imu.delta_ang_dt};
|
||||
odom.rollspeed = rates(0) - gyro_bias(0);
|
||||
odom.pitchspeed = rates(1) - gyro_bias(1);
|
||||
odom.yawspeed = rates(2) - gyro_bias(2);
|
||||
// velocity
|
||||
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
|
||||
_ekf.getVelocity().copyTo(odom.velocity);
|
||||
|
||||
// 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]);
|
||||
// angular_velocity
|
||||
const Vector3f rates{_ekf.get_imu_sample_newest().delta_ang / _ekf.get_imu_sample_newest().delta_ang_dt};
|
||||
const Vector3f angular_velocity = rates - _ekf.getGyroBias();
|
||||
angular_velocity.copyTo(odom.angular_velocity);
|
||||
|
||||
// Get covariances to vehicle odometry
|
||||
float covariances[24];
|
||||
_ekf.covariances_diagonal().copyTo(covariances);
|
||||
// velocity covariances
|
||||
_ekf.velocity_covariances().diag().copyTo(odom.velocity_variance);
|
||||
|
||||
// initially set pose covariances to 0
|
||||
for (size_t i = 0; i < POS_URT_SIZE; i++) {
|
||||
odom.pose_covariance[i] = 0.0;
|
||||
}
|
||||
// position covariances
|
||||
_ekf.position_covariances().diag().copyTo(odom.position_variance);
|
||||
|
||||
// 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];
|
||||
// orientation covariance
|
||||
_ekf.orientation_covariances_euler().diag().copyTo(odom.orientation_variance);
|
||||
|
||||
odom.reset_counter = _ekf.get_quat_reset_count()
|
||||
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
|
||||
+ _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();
|
||||
|
||||
odom.quality = 0;
|
||||
|
||||
// publish vehicle odometry data
|
||||
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_odometry_pub.publish(odom);
|
||||
@@ -1138,37 +1109,30 @@ void EKF2::PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_od
|
||||
vehicle_odometry_s aligned_ev_odom{ev_odom};
|
||||
|
||||
// Rotate external position and velocity into EKF navigation frame
|
||||
const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z);
|
||||
aligned_ev_odom.x = aligned_pos(0);
|
||||
aligned_ev_odom.y = aligned_pos(1);
|
||||
aligned_ev_odom.z = aligned_pos(2);
|
||||
const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position);
|
||||
aligned_pos.copyTo(aligned_ev_odom.position);
|
||||
|
||||
switch (ev_odom.velocity_frame) {
|
||||
case vehicle_odometry_s::BODY_FRAME_FRD: {
|
||||
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
|
||||
aligned_ev_odom.vx = aligned_vel(0);
|
||||
aligned_ev_odom.vy = aligned_vel(1);
|
||||
aligned_ev_odom.vz = aligned_vel(2);
|
||||
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: {
|
||||
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity);
|
||||
aligned_vel.copyTo(aligned_ev_odom.velocity);
|
||||
break;
|
||||
}
|
||||
|
||||
case vehicle_odometry_s::LOCAL_FRAME_FRD: {
|
||||
const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
|
||||
aligned_ev_odom.vx = aligned_vel(0);
|
||||
aligned_ev_odom.vy = aligned_vel(1);
|
||||
aligned_ev_odom.vz = aligned_vel(2);
|
||||
case vehicle_odometry_s::VELOCITY_FRAME_FRD: {
|
||||
const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity);
|
||||
aligned_vel.copyTo(aligned_ev_odom.velocity);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
|
||||
aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
|
||||
|
||||
// Compute orientation in EKF navigation frame
|
||||
Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ;
|
||||
Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q);
|
||||
ev_quat_aligned.normalize();
|
||||
|
||||
ev_quat_aligned.copyTo(aligned_ev_odom.q);
|
||||
quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
|
||||
|
||||
aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
|
||||
@@ -1636,77 +1600,122 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
|
||||
}
|
||||
|
||||
extVisionSample ev_data{};
|
||||
ev_data.pos.setNaN();
|
||||
ev_data.vel.setNaN();
|
||||
ev_data.quat.setNaN();
|
||||
|
||||
// if error estimates are unavailable, use parameter defined defaults
|
||||
|
||||
// check for valid velocity data
|
||||
if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
|
||||
ev_data.vel(0) = ev_odom.vx;
|
||||
ev_data.vel(1) = ev_odom.vy;
|
||||
ev_data.vel(2) = ev_odom.vz;
|
||||
if (PX4_ISFINITE(ev_odom.velocity[0]) && PX4_ISFINITE(ev_odom.velocity[1]) && PX4_ISFINITE(ev_odom.velocity[2])) {
|
||||
bool velocity_valid = true;
|
||||
|
||||
if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
|
||||
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
||||
switch (ev_odom.velocity_frame) {
|
||||
// case vehicle_odometry_s::VELOCITY_FRAME_NED:
|
||||
// ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
|
||||
// break;
|
||||
|
||||
} else {
|
||||
case vehicle_odometry_s::VELOCITY_FRAME_FRD:
|
||||
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
||||
break;
|
||||
|
||||
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
|
||||
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
||||
break;
|
||||
|
||||
default:
|
||||
velocity_valid = false;
|
||||
break;
|
||||
}
|
||||
|
||||
// velocity measurement error from ev_data or parameters
|
||||
float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
|
||||
if (velocity_valid) {
|
||||
ev_data.vel(0) = ev_odom.velocity[0];
|
||||
ev_data.vel(1) = ev_odom.velocity[1];
|
||||
ev_data.vel(2) = ev_odom.velocity[2];
|
||||
|
||||
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
|
||||
&& PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
|
||||
&& PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
|
||||
ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
|
||||
ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
|
||||
ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
|
||||
ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
|
||||
ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
|
||||
ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
|
||||
const float evv_noise_var = sq(_param_ekf2_evv_noise.get());
|
||||
|
||||
} else {
|
||||
ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
|
||||
// velocity measurement error from ev_data or parameters
|
||||
if (!_param_ekf2_ev_noise_md.get() &&
|
||||
PX4_ISFINITE(ev_odom.velocity_variance[0]) &&
|
||||
PX4_ISFINITE(ev_odom.velocity_variance[1]) &&
|
||||
PX4_ISFINITE(ev_odom.velocity_variance[2])) {
|
||||
|
||||
ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]);
|
||||
ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]);
|
||||
ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]);
|
||||
|
||||
} else {
|
||||
ev_data.velVar.setAll(evv_noise_var);
|
||||
}
|
||||
|
||||
new_ev_odom = true;
|
||||
}
|
||||
|
||||
new_ev_odom = true;
|
||||
}
|
||||
|
||||
// check for valid position data
|
||||
if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
|
||||
ev_data.pos(0) = ev_odom.x;
|
||||
ev_data.pos(1) = ev_odom.y;
|
||||
ev_data.pos(2) = ev_odom.z;
|
||||
if (PX4_ISFINITE(ev_odom.position[0]) && PX4_ISFINITE(ev_odom.position[1]) && PX4_ISFINITE(ev_odom.position[2])) {
|
||||
|
||||
float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
|
||||
bool position_valid = true;
|
||||
|
||||
// position measurement error from ev_data or parameters
|
||||
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
|
||||
&& PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
|
||||
&& PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
|
||||
ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
|
||||
ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
|
||||
ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
|
||||
// switch (ev_odom.pose_frame) {
|
||||
// case vehicle_odometry_s::POSE_FRAME_NED:
|
||||
// ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
|
||||
// break;
|
||||
|
||||
} else {
|
||||
ev_data.posVar.setAll(param_evp_noise_var);
|
||||
// case vehicle_odometry_s::POSE_FRAME_FRD:
|
||||
// ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// position_valid = false;
|
||||
// break;
|
||||
// }
|
||||
|
||||
if (position_valid) {
|
||||
ev_data.pos(0) = ev_odom.position[0];
|
||||
ev_data.pos(1) = ev_odom.position[1];
|
||||
ev_data.pos(2) = ev_odom.position[2];
|
||||
|
||||
const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
|
||||
|
||||
// position measurement error from ev_data or parameters
|
||||
if (!_param_ekf2_ev_noise_md.get() &&
|
||||
PX4_ISFINITE(ev_odom.position_variance[0]) &&
|
||||
PX4_ISFINITE(ev_odom.position_variance[1]) &&
|
||||
PX4_ISFINITE(ev_odom.position_variance[2])
|
||||
) {
|
||||
ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
|
||||
ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
|
||||
ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
|
||||
|
||||
} else {
|
||||
ev_data.posVar.setAll(evp_noise_var);
|
||||
}
|
||||
|
||||
new_ev_odom = true;
|
||||
}
|
||||
|
||||
new_ev_odom = true;
|
||||
}
|
||||
|
||||
// check for valid orientation data
|
||||
if (PX4_ISFINITE(ev_odom.q[0])) {
|
||||
if ((PX4_ISFINITE(ev_odom.q[0]) && PX4_ISFINITE(ev_odom.q[1])
|
||||
&& PX4_ISFINITE(ev_odom.q[2]) && PX4_ISFINITE(ev_odom.q[3]))
|
||||
&& ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f)
|
||||
|| (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f))
|
||||
) {
|
||||
ev_data.quat = Quatf(ev_odom.q);
|
||||
ev_data.quat.normalize();
|
||||
|
||||
// orientation measurement error from ev_data or parameters
|
||||
float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
|
||||
const float eva_noise_var = sq(_param_ekf2_eva_noise.get());
|
||||
|
||||
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
|
||||
ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
|
||||
if (!_param_ekf2_ev_noise_md.get() &&
|
||||
PX4_ISFINITE(ev_odom.orientation_variance[2])
|
||||
) {
|
||||
ev_data.angVar = fmaxf(eva_noise_var, ev_odom.orientation_variance[2]);
|
||||
|
||||
} else {
|
||||
ev_data.angVar = param_eva_noise_var;
|
||||
ev_data.angVar = eva_noise_var;
|
||||
}
|
||||
|
||||
new_ev_odom = true;
|
||||
@@ -1714,6 +1723,8 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
|
||||
|
||||
// use timestamp from external computer, clocks are synchronized when using MAVROS
|
||||
ev_data.time_us = ev_odom.timestamp_sample;
|
||||
ev_data.reset_counter = ev_odom.reset_counter;
|
||||
//ev_data.quality = ev_odom.quality;
|
||||
|
||||
if (new_ev_odom) {
|
||||
_ekf.setExtVisionData(ev_data);
|
||||
|
||||
Reference in New Issue
Block a user