mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-30 19:20: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
@@ -207,7 +207,7 @@ struct extVisionSample {
|
||||
Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
|
||||
Quatf quat{}; ///< quaternion defining rotation from body to earth frame
|
||||
Vector3f posVar{}; ///< XYZ position variances (m**2)
|
||||
Matrix3f velCov{}; ///< XYZ velocity covariances ((m/sec)**2)
|
||||
Vector3f velVar{}; ///< XYZ velocity variances ((m/sec)**2)
|
||||
float angVar{}; ///< angular heading variance (rad**2)
|
||||
VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
||||
uint8_t reset_counter{};
|
||||
|
||||
@@ -201,6 +201,8 @@ public:
|
||||
// get the orientation (quaterion) covariances
|
||||
matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); }
|
||||
|
||||
matrix::SquareMatrix<float, 3> orientation_covariances_euler() const;
|
||||
|
||||
// get the linear velocity covariances
|
||||
matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<3, 3>(4, 4); }
|
||||
|
||||
|
||||
@@ -1508,7 +1508,7 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
|
||||
|
||||
Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
|
||||
{
|
||||
Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
|
||||
Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar);
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch (_ev_sample_delayed.vel_frame) {
|
||||
@@ -1903,3 +1903,60 @@ void Ekf::resetGpsDriftCheckFilters()
|
||||
_gps_vertical_position_drift_rate_m_s = NAN;
|
||||
_gps_filtered_horizontal_velocity_m_s = NAN;
|
||||
}
|
||||
|
||||
matrix::SquareMatrix<float, 3> Ekf::orientation_covariances_euler() 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 = _state.quat_nominal(0);
|
||||
float q2 = _state.quat_nominal(1);
|
||||
float q3 = _state.quat_nominal(2);
|
||||
float q4 = _state.quat_nominal(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;
|
||||
|
||||
if (fabsf(d1) < FLT_EPSILON) {
|
||||
d1 = FLT_EPSILON;
|
||||
}
|
||||
|
||||
if (fabsf(d2) < FLT_EPSILON) {
|
||||
d2 = FLT_EPSILON;
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
const matrix::SquareMatrix<float, 4> quat_covariances = P.slice<4, 4>(0, 0);
|
||||
|
||||
return G * quat_covariances * G.transpose();
|
||||
}
|
||||
|
||||
@@ -245,6 +245,8 @@ public:
|
||||
|
||||
// Getters for samples on the delayed time horizon
|
||||
const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
|
||||
const imuSample &get_imu_sample_newest() const { return _newest_high_rate_imu_sample; }
|
||||
|
||||
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
|
||||
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
|
||||
|
||||
|
||||
+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);
|
||||
|
||||
@@ -144,7 +144,7 @@ private:
|
||||
void PublishInnovationTestRatios(const hrt_abstime ×tamp);
|
||||
void PublishInnovationVariances(const hrt_abstime ×tamp);
|
||||
void PublishLocalPosition(const hrt_abstime ×tamp);
|
||||
void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu);
|
||||
void PublishOdometry(const hrt_abstime ×tamp);
|
||||
void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom);
|
||||
void PublishOpticalFlowVel(const hrt_abstime ×tamp);
|
||||
void PublishSensorBias(const hrt_abstime ×tamp);
|
||||
|
||||
@@ -26,12 +26,7 @@ void Vio::setData(const extVisionSample &vio_data)
|
||||
|
||||
void Vio::setVelocityVariance(const Vector3f &velVar)
|
||||
{
|
||||
setVelocityCovariance(matrix::diag(velVar));
|
||||
}
|
||||
|
||||
void Vio::setVelocityCovariance(const Matrix3f &velCov)
|
||||
{
|
||||
_vio_data.velCov = velCov;
|
||||
_vio_data.velVar = velVar;
|
||||
}
|
||||
|
||||
void Vio::setPositionVariance(const Vector3f &posVar)
|
||||
@@ -76,7 +71,7 @@ extVisionSample Vio::dataAtRest()
|
||||
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
|
||||
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
|
||||
vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
|
||||
vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
|
||||
vio_data.angVar = 0.05f;
|
||||
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
||||
return vio_data;
|
||||
|
||||
@@ -53,7 +53,6 @@ public:
|
||||
|
||||
void setData(const extVisionSample &vio_data);
|
||||
void setVelocityVariance(const Vector3f &velVar);
|
||||
void setVelocityCovariance(const Matrix3f &velCov);
|
||||
void setPositionVariance(const Vector3f &posVar);
|
||||
void setAngularVariance(float angVar);
|
||||
void setVelocity(const Vector3f &vel);
|
||||
|
||||
@@ -276,13 +276,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
|
||||
// WHEN: measurement is given in BODY-FRAME and
|
||||
// x variance is bigger than y variance
|
||||
_sensor_simulator._vio.setVelocityFrameToBody();
|
||||
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.01f, 0.0f,
|
||||
0.0f, 0.0f, 0.01f
|
||||
};
|
||||
const Matrix3f vel_cov_body(vel_cov_data);
|
||||
|
||||
const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
|
||||
const Vector3f vel_body(1.0f, 0.0f, 0.0f);
|
||||
_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
|
||||
_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
|
||||
_sensor_simulator._vio.setVelocity(vel_body);
|
||||
_ekf_wrapper.enableExternalVisionVelocityFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
@@ -312,13 +309,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
|
||||
// WHEN: measurement is given in LOCAL-FRAME and
|
||||
// x variance is bigger than y variance
|
||||
_sensor_simulator._vio.setVelocityFrameToLocal();
|
||||
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
|
||||
0.0f, 0.01f, 0.0f,
|
||||
0.0f, 0.0f, 0.01f
|
||||
};
|
||||
const Matrix3f vel_cov_earth(vel_cov_data);
|
||||
|
||||
const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f};
|
||||
const Vector3f vel_earth(1.0f, 0.0f, 0.0f);
|
||||
_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
|
||||
_sensor_simulator._vio.setVelocityVariance(vel_cov_earth);
|
||||
_sensor_simulator._vio.setVelocity(vel_earth);
|
||||
_ekf_wrapper.enableExternalVisionVelocityFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
|
||||
Reference in New Issue
Block a user