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 ID: 650184
Download Presentation The PPT/PDF document "Using Sensor Data Effectively" is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
Slide1
Using Sensor Data Effectively
A major challenge in mobile robotics is accurate localisation.Consider a robot with the following sensors:Inertial Measurement Unit (IMU)CameraWheel EncodersGlobal PositioningEach has different noise properties.
LocalizationSlide2
No noise
, perfect signal
5% noise
15% noise
20% noise
Time
State
Sensor Noise
LocalizationSlide3
Kalman
FilterStateTime
True State
Noisy Sensor Reading
EKF Filtered
LocalizationSlide4
Theory:
Kalman Filter Provably optimal when noise is Gaussian distributed with zero mean.And the control and measurement models are linear.
P(noise)
noise
LocalizationSlide5
Linear Control and Observation
Observation Model:
Control (transition) Model:
LocalizationSlide6
Relating Control
, Measurement and Noise
LocalizationSlide7
Relating Control
, Measurement and Noise
LocalizationSlide8
Extended Kalman
FilterThe 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.
LocalizationSlide9
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.
LocalizationSlide10
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.
LocalizationSlide11
The EKF Algorithm
LocalizationSlide12
The EKF Algorithm
Input previous state estimate
LocalizationSlide13
The EKF Algorithm
Output an estimate for the current state
Input previous state estimate
LocalizationSlide14
The EKF Algorithm
Previous covariance
LocalizationSlide15
The EKF Algorithm
… and the updated covariance matrix.
Previous covariance
LocalizationSlide16
The EKF Algorithm
Current Observation
LocalizationSlide17
The EKF Algorithm
The
Kalman
gain, , weights the sensor measurements according to, , the prediction covariance (noisiness).
LocalizationSlide18
The EKF Algorithm
The new observation coefficient matrix depends on how good the previous one was.
LocalizationSlide19
The EKF Algorithm
Prediction Step
LocalizationSlide20
The EKF Algorithm
Prediction Step
Correction
Step
LocalizationSlide21
Sensor Fusion
home.wlu.edu/~levys/kalman_tutorial/
Sensor 2: Lower Covariance
Sensor 1:
Higher Covariance
Ground Truth
EKF Fusion
The EKF estimate is closer to the sensor with
less noise.
LocalizationSlide22
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.
LocalizationSlide23
EKF in ROS
Two nodes:ekf_localization: EKF implementationNavsat node: sensor processing. Transforms latitude and longitude to the robots local frame.
LocalizationSlide24
EKF localization node
Topic:
odom
/
navsat
Type:
odom
with covariance
Topic:
odom
Type:
odomWheel encoders
Navsat
transform node
Topic:
imu
Type:
imu
Topic: fix
Type:
navsat
/
fix
Topic:
odom
/
ekf
Type:
odometry
with covariance
LocalizationSlide25
EKF Node Basic Configuration
<rosparam param="imu0_config"> [false, false, false, true, true, true, false, false, false,
true, true, true,
true, true, true]
</
rosparam
>
<rosparam
param="odom0_config">
[true, true, false, false, false, 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
.
In
~/
rover_workspace
/launch/
rover_name.launch
LocalizationSlide26
EKF Node Basic
ConfigurationLocalizationSlide27
EKF Node Basic
ConfigurationLocalization
Has to be tuned through experiment.Slide28
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