1. 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

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

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

4. 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

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

6. Relating Control, Measurement and Noise
Localization

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

8. 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

9. 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

10. 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

11. The EKF Algorithm
Localization

12. Input previous state estimate
The EKF Algorithm
Localization

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

14. Previous covariance
The EKF Algorithm
Localization

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

16. Current Observation
The EKF Algorithm
Localization

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

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

19. The EKF Algorithm
Prediction Step
Localization

20. The EKF Algorithm
Prediction Step
Correction
Step
Localization

21. 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/

22. 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 “A Generalized Extended Kalman Filter Implementation for the Robot Operating System.” In Intelligent Autonomous Systems 13, 335–48. Springer.
Localization

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

24. 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

25. 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

26. EKF Node Basic Configuration
Localization

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

28. 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