Use EKF for filtered odometry

[5yler]

Branched off from Issue #9. I think that in order to use ekf_localization_node in the odom frame, we would have to disable the odom->base_link transform broadcast by odom_tf_publisher, which currently both:

• publishes to the odom topic
• broadcasts the transform odom->base_link

See Using robot_localization with amcl (ROS Answers)

Tasks

• [ ] add parameter to odom_tf_publisher to disable broadcasting odom->base_link transform
• [ ] run ekf_odom.launch with modified odom_tf_publisher and collect rosbag data
• [ ] compare accuracy of odom->base_link transform with previous results

[5yler]

Initial results with ekf_odom.launch on a rosbag filtered to exclude tf messages, with raw odometry in yellow and odom/ekf in turquoise:

[5yler]

Changed

-      <rosparam param="odom0_config">[true,  true, false,   <!-- x, y, z -->
-                                      false, false, true,   <!-- roll, pitch, yaw -->

+      <rosparam param="odom0_config">[false, false, false,  <!-- x, y, z -->
+                                      false, false, false,  <!-- roll, pitch, yaw -->

       <rosparam param="imu0_config">[false, false, false, <!-- x, y, z -->
-                                     true,  true,  true,  <!-- roll, pitch, yaw -->
+                                     false, false, true,  <!-- roll, pitch, yaw -->
                                      false, false, false, <!-- vx, vy, vz -->
-                                     true,  true,  true,  <!-- vroll, vpitch, vyaw -->
-                                     true,  true,  true]  <!-- ax, ay, az -->
+                                     false, false, true,  <!-- vroll, vpitch, vyaw -->
+                                     true,  true,  false]  <!-- ax, ay, az -->

-      <param name="imu0_differential" value="false"/> 
+      <param name="imu0_differential" value="true"/>

[5yler]

This started out looking good, but then the odom/ekf really really drifted:

Not sure if we can get accurate filtered odometry given that our IMU does not output covariance data.

[5yler]

Some relevant links for estimating covariances:

• How to calculate covariance matrix? [ROS Answers]
• calculating covariances of robot's odometry ? [ROS Answers]

[5yler]

On Tue, Oct 18, 2016 at 4:27 AM, DGonz wrote:

Covariance = (standard deviation of noise)^2

If we can record our sensor inputs while stationary, then plot the values in a histogram, this should give us a good idea for our sensor noise standard deviation, and thus our covariances.

Alternatively, if any of our sensors' datasheets include the average noise profile of their devices, it will certainly be good enough to use as the noise covariance.