Autonomous Systems: Tutorial 6

Localization: Basics of Kalman Filter

Dr. Islam S. M. Khalil

German University in Cairo

Faculty of Engineering and Material Science

May 26, 2018

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 1 / 19

Navigation Steps

Input from sensors; mobile robots are equipped with

incremental encoders that measure the rotation the wheels but
not the position and orientation of the robot.
1) Perception: the robot must interpret its sensors to extract
meaningful data.
2) Localization: the robot must determine its position in the
environment/ is a procedure for calculating the robot
configuration q in real time.
3) Cognition: the robot must decide how to act to achieve its
goals.
4) Motion Control: the robot must modulate its motor outputs
to achieve the desired trajectory.
Output to the actuator.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 2 / 19

Localization

Figure: Integration of localization, cognition, control, and perception.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 3 / 19

Localization - differential mobile robot
     
ẋ cos θ 0
ẏ  =  sin θ  v + 0 ω (1)
θ̇ 0 1

Equation 1 shows the model for a differential drive,

nonlinear representation ẋ = g1 u1 + g2 u2 .
Velocity inputs v and ω are assumed to be available all the
time.
Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 4 / 19
Localization - differential mobile robot

Real systems depends on discretized model due to the time

taken in the processing time of the code and receiving the
signals from the sensors.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 5 / 19

Localization - differential mobile robot

1) Euler integration.
dx ∆x x − xk
ẋ = = = k+1 = vk cos θk
dt ∆t Ts
where Ts = tk+1 − tk is the sampling time. Rearrange the
equation to get:
xk+1 = xk + vk Ts cos θk
and similar steps for y and θ.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 6 / 19

Localization - differential mobile robot

xk+1 = xk + vk Ts cos θk
yk+1 = yk + vk Ts sin θk (2)
θk+1 = θk + ωk Ts

Further xk+1 and yk+1 are approximate, and θk+1 is exact.

2) Runge-Kutta integration.
 
ωk Ts
xk+1 = xk + vk Ts cos θk +
2
 
ω Ts
yk+1 = yk + vk Ts sin θk + k (3)
2
θk+1 = θk + ωk Ts

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 7 / 19

Localization - differential mobile robot

In Runge-Kutta integration: the average orientation during

[tk , tk+1 ], where ωk Ts = ∆θ. xk+1 and yk+1 are still approximate
but more accurate.
3) Exact integration.

vk
xk+1 = xk + (sin θk+1 − sin θk )
ωk
v
yk+1 = yk − k (cos θk+1 − cos θk ) (4)
ωk
θk+1 = θk + ωk Ts

For ωk = 0, xk+1 and yk+1 are still defined and coincide with the
solution of Euler and Runge-Kutta.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 8 / 19

Localization- Geometric comparison

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 9 / 19

Localization - differential drive robots

Odometric localization or dead

reckoning

Encoders and IMU are recording

and giving totally different path
than initial path.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 10 / 19

Localization - differential drive robots

Odometric position updates can give only a very rough

estimate of the actual position.
There are inaccuracies due to two different reasons:
1) Uncertainty in sensors: i.e encoders, ultrasonic, .... etc.
leading to inaccuracy in state measurement.
2) Uncertainty in actuation: different velocities from motors
although same PWM is sent, due to deformation of wheel,
slippage, unequal floor and so on.
Therefore, state estimators (predictors) are used.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 11 / 19

Kalman Filter

Moving in x-direction:

Predicted state: using the discretized model.

Measured value: using the encoders.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 12 / 19

Kalman Filter

Based on kalman gain, which one should I depend on it more?

1) Kalman Gain:
error(Estimated)
KG =
error(Estimated) − error(measured)

KG : 0
|{z} → 1
|{z}
Estimated measurement
is very accurate is very accurate

2) Update the state estimation:

xk+1 = xk + KG[Measured − Estimated]

e.g.
= 15 + 0.7[16 − 15] = 15.7

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 13 / 19

Kalman Filter

3) Update the error estimation:

error(Estimated)k +1 = [1 − KG]error(Estimated)k

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 14 / 19

Kalman Filter approach

Two Stages:
First Stage: Prediction: generate an intermediate estimate
x
bk+1|k by propagating x bk using the process dynamics (Steps 1,
2).
Second Stage: Correction (update): Correct the prediction on
the basis of the difference between the measured and the
predicted output (Steps 3, 4, 5).
1) Consider a linear discrete-time model

x = Ak xk + Bk uk
| k+1
b
{z } |{z}
estimated / control
predicted input
state

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 15 / 19

Kalman Filter approach
2)
bk+1 = APk AT +
P Q
| {z } |{z}
predicted process
error covariance
covariance matrix
matrix

Q exist due to the variance of the output velocity between the

left and the right wheel although having same input.
 2 
αr 0
Q=
0 αl2
Prediction error should be
decreasing with iterations as
shown in the diagram.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 16 / 19

Kalman Filter approach

3) Kalman Gain:
bk+1 H T + R)−1
bk+1 H T (H P
KGk+1 = P

H T relevant to C, R is the measurement covariance matrix

due to the uncertainty in the sensors e.g. deviation in the
encoders.  2 
σr 0
R=
0 σl2

4) Update the state estimate:

xk+1 = xbk+1 + KGk+1 (zk+1 − H xbk +1 )

where xbk+1 is the predicted state, zk+1 is the measurement

value and xbk+1 is the estimated.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 17 / 19

Kalman Filter approach

5) Update the error covariance matrix:

Pk+1 = [I − KGk+1 H]P

bk+1

This approach is valid for linear system, in order to be applied

on nonlinear systems, use extended Kalman Filter.

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 18 / 19

 Any Questions ?
Thank you

Dr. Islam S. M. Khalil Autonomous Systems: Tutorial 6 19 / 19