dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
kritz 2019-09-23 20:24:52 +02:00 committed by Kabir Mohammed
parent 412b44ff6e
commit 7427768e70
4 changed files with 60 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/vehicle_odometry.msg
View File

@ -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

74
src/modules/ekf2/ekf2_main.cpp
View File

@ -255,6 +255,10 @@ private:
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 @@ private:
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;
@ -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();

2
src/modules/logger/logger.cpp
View File

@ -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");
}

2
src/modules/mavlink/mavlink_receiver.cpp
View File

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