Working with the robot_localization Package
Tom Moore
Clearpath Robotics

What is robot_localization?
• General purpose state estimation package• No limit on the number of input data sources• Supported message types for state estimation nodes• nav_msgs/Odometry• geometry_msgs/PoseWithCovarianceStamped• geometry_msgs/TwistWithCovarianceStamped• sensor_msgs/Imu• State vector: [ x y z α• Two typical use cases

β γx˙y˙z˙ α˙β˙ γ˙˙x˙]˙y˙ ˙z˙• Fuse continuous sensor data (e.g., wheel encoder odometry and IMU) to€ locally accurate state estimate
produce• Fuse continuous data with global pose estimates (e.g., from SLAM) toprovide an accurate and complete global state estimate

NodesState estimation nodes• ekf_localization_node – Implementation of an extended Kalman filter (EKF)• ukf_localization_node – Implementation of an unscented Kalman filter (UKF)

Sensor preprocessing nodes• navsat_transform_node – Allows users to easily transform geographic
coordinates (latitude and longitude) into the robot’s world frame (typicallymap or odom)	  robot_localization and the ROS
Navigation Stackamcl	  gmapping	  navsat_transform_node	  GPS	  IMU	  Wheel	  Encoders	  Visual	  Odometry	  Barometer	  And	  so	  much	  more!	  

*kf_localiza,on_node	  *kf_localiza,on_node	  Source: http://wiki.ros.org/move_base

Preparing Your Sensor Data: REPs
Fact: you can’t spell “prepare” without REP.
Two important REPs

• REP-103: http://www.ros.org/reps/rep-0103.html• Covers standards for units and basic coordinate frame conventions• REP-105: http://www.ros.org/reps/rep-0105.html• Covers naming and semantics of the “principal” coordinate frames in ROS

Preparing Your Sensor Data: Transforms

map
ekf_localization_node
pose dataodom
ekf_localization_node

twist data** and IMU data

base_linkPreparing Your Sensor Data: Frame IDs

Requires abase_link! imu_link
Notransforms
requiredtransform

Preparing Your Sensor Data: Covarianceinitial_estimate_covariance (P0)	  process_noise_covariance	  ˆ k| k −1 = f ( xˆ k −1| k −1 )x

Pk| k −1 = JPk −1| k −1JT + Q€

K k = Pk| k −1HTk (HTk Pk| k −1HTk + R k )−1

ˆ k| k = x
ˆ k| k −1 + K k ( z k − H k x
ˆ k| k −1 )x
TPk| k = (I − K k H k )Pk| k −1 (I − K k H k ) + K k R k K Tk
nav_msgs/Odometrygeometry_msgs/PoseWithCovarianceStamped
geometry_msgs/TwistWithCovarianceStamped€
sensor_msgs/Imu

Configuring *kf_localization_node
Coordinate frame specification<param name="map_frame" value="map"/><param name="odom_frame" value="odom"/><param name="base_link_frame" value="base_link"/><param name="world_frame" value="odom"/>Configuring *kf_localization_node
Input specification<param name="odom0" value="/controller/odom"/><param name="odom1" value="/some/other/odom"/><param name="pose0" value="/altitude"/><param name="pose1" value="/some/other/pose"/><param name="pose2" value="/yet/another/pose"/><param name="twist0" value="/optical_flow"/><param name="imu0" value="/imu/left"/><param name="imu1" value="/imu/right"/><param name="imu2" value="/imu/front"/><param name="imu3" value="/imu/back"/>

Configuring *kf_localization_node
Basic input configuration<rosparam param="odom0_config">[true, true, false, x, y, z	  false, false, false, roll, pitch, yaw	  false, false, false, y velocity, y velocity, z velocity	  false, false, true, roll velocity, pitch velocity, yaw velocity	  false, false, false] x accel., y accel., z accel.	  
</rosparam><rosparam param="imu0_config">
param="odom1_config">[true, false,[false,true, false,
true,false, true,true,false, true,false,false, false, false,false, true,true,false, true,false,
false, true,true,false, true]
false]</rosparam><param name="odom1_differential" value="true”>

Configuring *kf_localization_node
Covariance specification (P0 and Q)<rosparam param="initial_estimate_covariance">[0.8, 0, ..., 1e-9]</rosparam><rosparam param="process_noise_covariance">[0.04, 0, ..., 0.02]</rosparam>

Live Demo

Using navsat_transform_node
What does it do?

• Many robots operate outdoors and make use of GPS receivers• Problem: getting the data into your robot’s world frame• Solution:• Convert GPS data to UTM coordinates• Use initial UTM coordinate, EKF/UKF output, and IMU to generate a(static) transform T from the UTM grid to your robot’s world frame• Transform all future GPS measurements using T• Feed output back into EKF/UKF

Required Inputs• nav_msgs/Odometry (EKF output, needed for robot’s current pose)• sensor_msgs/Imu (must have a compass, needed to determine global heading)• sensor_msgs/NavSatFix (output from your navigation satellite device)Using navsat_transform_node
Relevant settings<param name="magnetic_declination_radians" value="0"/><param name="yaw_offset" value="0"/><param name="zero_altitude" value="true"/><param name="broadcast_utm_transform" value="true"/><param name="publish_filtered_gps" value="true"/>

Using navsat_transform_node
Typical Setup(Sensor data)/imu/data(sensor_msgs/Imu)/gps/fix(sensor_msgs/NavSatFix)(Sensor data)*kf_localization_node (map)/odometry/filtered(/nav_msgs/Odometry)/odometry/gps(/nav_msgs/Odometry)navsat_transform_node*kf_localization_node (odom)

robot_localization in the Wild
robot_localization works on a broad range of robots! 	  

From	  this…	  (ayrbot)	  …to	  this	  (OTTO)	  FinThank you!

http://wiki.ros.org/robot_localization
tmoore@clearpathrobotics.com