From 7427768e70810676804f5cbed2060baa52e70945 Mon Sep 17 00:00:00 2001 From: kritz Date: Mon, 23 Sep 2019 20:24:52 +0200 Subject: [PATCH] ECL reference frame alignment fix (#12771) * 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 --- msg/vehicle_odometry.msg | 5 +- src/modules/ekf2/ekf2_main.cpp | 74 ++++++++++++++++++------ src/modules/logger/logger.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 4 files changed, 60 insertions(+), 23 deletions(-) diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 52db320e49f7..cbc3bca7bce2 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -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. @@ -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 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 31402bc82dc6..bf582314e876 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -255,6 +255,10 @@ class Ekf2 final : public ModuleBase, 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)}; @@ -287,6 +291,7 @@ class Ekf2 final : public ModuleBase, public ModuleParams uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::PublicationData _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; Ekf _ekf; @@ -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(); @@ -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(); @@ -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); } @@ -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(); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 6bfa28a32ebe..8bd3f210c2b6 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -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"); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d22333e955ff..2dd150151848 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 @@ -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 linvel_local(R_body_to_local * matrix::Vector3(odom.vx, odom.vy, odom.vz)); + odometry.vx = linvel_local(0); odometry.vy = linvel_local(1); odometry.vz = linvel_local(2);