Using Sensor Data Effectively A major challenge in mobile robotics is accurate localisation. Consider a robot with the following sensors: Inertial Measurement Unit (IMU) Camera Wheel Encoders Global Positioning Each has different noise properties. Localization

Sensor Noise State Time No noise, perfect signal 5% noise 20% noise Localization

Kalman Filter State Time True State Noisy Sensor Reading EKF Filtered Localization

Theory: Kalman Filter Provably optimal when noise is Gaussian distributed with zero mean. And the control and measurement models are linear. P(noise) noise Localization

Linear Control and Observation Control (transition) Model: Observation Model: Localization

Relating Control, Measurement and Noise Localization

Relating Control, Measurement and Noise Q and R are the covariance matricies that define the gaussian random variables epsilon and delta. Localization

Extended Kalman Filter The Kalman filter assumed linear relationships. In real systems the relationship may be non-linear. But we can take the derivative of the signal to linearize it. This is the extended Kalman filter. No longer optimal but works well. Localization

Sensor Fusion with Multiple States Recall the sensor measurement is: We would like to generalise this to multiple sensors with different kinds of states: For example, states might be yaw-angle, velocity, or position. Sensors might be IMU, encoders, or GPS. Localization

Sensor Fusion with Multiple States Recall the sensor measurement is: If we have three sensors with 2 different states we can write: With each row corresponding to a sensor and each column to a type of state. Localization

The EKF Algorithm Localization

Input previous state estimate The EKF Algorithm Localization

The EKF Algorithm Input previous state estimate Output an estimate for the current state Localization

Previous covariance The EKF Algorithm Localization

The EKF Algorithm Previous covariance … and the updated covariance matrix. Localization

Current Observation The EKF Algorithm Localization

The Kalman gain, , weights the sensor measurements according to, , the prediction covariance (noisiness). The EKF Algorithm Localization

The EKF Algorithm The new observation coefficient matrix depends on how good the previous one was. Localization

The EKF Algorithm Prediction Step Localization

The EKF Algorithm Prediction Step Correction Step Localization

Sensor Fusion Sensor 1: Higher Covariance Ground Truth Sensor 2: Lower Covariance EKF Fusion The EKF estimate is closer to the sensor with less noise. Localization home.wlu.edu/~levys/kalman_tutorial/

In Practice The EKF is a recursive function that calls itself at each time step to improve the robot state estimate. It weights sensor data by the variance they contribute to the state prediction. ROS has an EKF package to do all this for us* *Moore, Thomas, and Daniel Stouch. 2016. “A Generalized Extended Kalman Filter Implementation for the Robot Operating System.” In Intelligent Autonomous Systems 13, 335–48. Springer. Localization

EKF in ROS Two nodes: ekf_localization: EKF implementation Navsat node: sensor processing. Transforms latitude and longitude to the robots local frame. Localization

Navsat transform node EKF localization node Topic: fix Type: navsat/fix Topic: imu Type: imu Navsat transform node Topic: odom/navsat Type: odom with covariance Topic: odom Type: odom Wheel encoders GPS EKF localization node Topic: odom/ekf Type: odometry with covariance Localization

EKF Node Basic Configuration <rosparam param="odom0_config"> [true, true, false, false, false, false, false, false, true, false, false, false] </rosparam> x, y, z roll, pitch, yaw y velocity, y velocity, z velocity roll velocity, pitch velocity, yaw velocity x accel., y accel., z accel. <rosparam param="imu0_config"> [false, false, false, true, true, true, false, false, false, true, true, true] </rosparam> In ~/rover_workspace/launch/rover_name.launch Localization

EKF Node Basic Configuration Localization

EKF Node Basic Configuration Has to be tuned through experiment. Localization

In Practice Can set initial values for, , the prediction covariance matrix. Small initial values tell the Robot that the sensor has low noise. High values say that the sensor is noisy. Ideally the covariance is measured experimentally for each sensor. The initial value for can be the first sensor value. That is . Localization