Skip to content

Commit

Permalink
ECL reference frame alignment fix (PX4#12771)
Browse files Browse the repository at this point in the history 
* Fix EKF frame alignemen in ECL

* Remove empty lines

* Add initalization for ev_odom

* Only use yaw covariance for angErr

* Improve frame naming in comments

* Use copyTo

* Add aligned as suffix

* Add missing vehicle_visual_odometry_aligned
  • Loading branch information
@kamilritz @mhkabir
kamilritz authored and mhkabir committed Sep 23, 2019
1 parent 412b44f commit 7427768
Show file tree
Hide file tree
Showing 4 changed files with 60 additions and 23 deletions.
5 changes: 3 additions & 2 deletions msg/vehicle_odometry.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,8 @@ float32 y # East position
float32 z # Down position

# Orientation quaternion. First value NaN if invalid/unknown
float32[4] q # Quaternion rotation from NED earth-fixed frame to XYZ body frame
float32[4] q # Quaternion rotation from FRD body frame to refernce frame
float32[4] q_offset # Quaternion rotation from odometry reference frame to navigation frame

# Row-major representation of 6x6 pose cross-covariance matrix URT.
# NED earth-fixed frame.
Expand All @@ -55,4 +56,4 @@ float32 yawspeed # Angular velocity about Z body axis
# If angular velocity covariance invalid/unknown, 16th cell is NaN
float32[21] velocity_covariance

# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned
74 changes: 55 additions & 19 deletions src/modules/ekf2/ekf2_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,10 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84

// republished aligned external visual odometry
bool new_ev_data_received = false;
vehicle_odometry_s _ev_odom{};

uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
Expand Down Expand Up @@ -287,6 +291,7 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};

Ekf _ekf;

Expand Down Expand Up @@ -1139,26 +1144,29 @@ void Ekf2::run()

// get external vision data
// if error estimates are unavailable, use parameter defined defaults
new_ev_data_received = false;

if (_ev_odom_sub.updated()) {
new_ev_data_received = true;

// copy both attitude & position, we need both to fill a single ext_vision_message
vehicle_odometry_s ev_odom;
_ev_odom_sub.copy(&ev_odom);
_ev_odom_sub.copy(&_ev_odom);

ext_vision_message ev_data;

// check for valid position data
if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
ev_data.posNED(0) = ev_odom.x;
ev_data.posNED(1) = ev_odom.y;
ev_data.posNED(2) = ev_odom.z;
if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) {
ev_data.posNED(0) = _ev_odom.x;
ev_data.posNED(1) = _ev_odom.y;
ev_data.posNED(2) = _ev_odom.z;

// position measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {
// position measurement error from parameter
if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {
ev_data.posErr = fmaxf(_param_ekf2_evp_noise.get(),
sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
ev_data.hgtErr = fmaxf(_param_ekf2_evp_noise.get(),
sqrtf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]));
sqrtf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]));

} else {
ev_data.posErr = _param_ekf2_evp_noise.get();
Expand All @@ -1167,15 +1175,13 @@ void Ekf2::run()
}

// check for valid orientation data
if (PX4_ISFINITE(ev_odom.q[0])) {
ev_data.quat = matrix::Quatf(ev_odom.q);
if (PX4_ISFINITE(_ev_odom.q[0])) {
ev_data.quat = matrix::Quatf(_ev_odom.q);

// orientation measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE])) {
if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE])) {
ev_data.angErr = fmaxf(_param_ekf2_eva_noise.get(),
sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_PITCH_VARIANCE],
ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]))));
sqrtf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]));

} else {
ev_data.angErr = _param_ekf2_eva_noise.get();
Expand All @@ -1185,10 +1191,10 @@ void Ekf2::run()
// only set data if all positions and orientation are valid
if (ev_data.posErr < ep_max_std_dev && ev_data.angErr < eo_max_std_dev) {
// use timestamp from external computer, clocks are synchronized when using MAVROS
_ekf.setExtVisionData(ev_odom.timestamp, &ev_data);
_ekf.setExtVisionData(_ev_odom.timestamp, &ev_data);
}

ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)_ev_odom.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}

Expand Down Expand Up @@ -1424,6 +1430,36 @@ void Ekf2::run()
// publish vehicle odometry data
_vehicle_odometry_pub.publish(odom);

// publish external visual odometry after fixed frame alignment if new odometry is received
if (new_ev_data_received) {
float q_ev2ekf[4];
_ekf.get_ev2ekf_quaternion(q_ev2ekf); // rotates from EV to EKF navigation frame
Quatf quat_ev2ekf(q_ev2ekf);
Dcmf ev_rot_mat(quat_ev2ekf);

vehicle_odometry_s aligned_ev_odom = _ev_odom;

// Rotate external position and velocity into EKF navigation frame
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);

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);

// Compute orientation in EKF navigation frame
Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(_ev_odom.q) ;
ev_quat_aligned.normalize();

ev_quat_aligned.copyTo(aligned_ev_odom.q);
quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);

_vehicle_visual_odometry_aligned_pub.publish(aligned_ev_odom);
}

if (_ekf.global_position_is_valid() && !_preflt_fail) {
// generate and publish global position data
vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
Expand Down
2 changes: 1 addition & 1 deletion src/modules/logger/logger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -615,7 +615,7 @@ void Logger::add_estimator_replay_topics()
add_topic("vehicle_magnetometer");
add_topic("vehicle_status");
add_topic("vehicle_visual_odometry");

add_topic("vehicle_visual_odometry_aligned");
add_topic_multi("distance_sensor");
add_topic_multi("vehicle_gps_position");
}
Expand Down
2 changes: 1 addition & 1 deletion src/modules/mavlink/mavlink_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1242,7 +1242,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.pose_covariance[i] = odom.pose_covariance[i];
}


/*
* PX4 expects the body's linear velocity in the local frame,
* the linear velocity is rotated from the odom child_frame to the
Expand All @@ -1256,6 +1255,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)

/* the linear velocities needs to be transformed to the local NED frame */
matrix::Vector3<float> linvel_local(R_body_to_local * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));

odometry.vx = linvel_local(0);
odometry.vy = linvel_local(1);
odometry.vz = linvel_local(2);
Expand Down

0 comments on commit 7427768

Please sign in to comment.