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 14:24:52 -04:00
committed by Kabir Mohammed
parent 412b44ff6e
commit 7427768e70
4 changed files with 60 additions and 23 deletions
+1 -1
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);