fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 10:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ECL reference frame alignment fix (#12771)

Browse Source 
* 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+55 -19
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();