Scribd
Upload
40 views

Autonomous Systems Tutorial 6

This document discusses localization for autonomous systems using the Kalman filter. It begins by explaining the navigation steps an autonomous robot takes, including perception, localization, cognition, and motion control. It then discusses challenges with localization using odometry alone due to sensor and actuation uncertainties. The Kalman filter is introduced as a way to estimate the true state by combining predictions with measurements. The summary discusses: 1) Localization is determining a robot's position and is challenging due to sensor and wheel inaccuracies over time. 2) The Kalman filter fuses predictions with measurements to estimate the true state, reducing errors over time. 3) It works in two stages - prediction using a model and correction by updating the prediction based

Uploaded by

Khaled Zaki
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
40 views

Autonomous Systems Tutorial 6

This document discusses localization for autonomous systems using the Kalman filter. It begins by explaining the navigation steps an autonomous robot takes, including perception, localization, cognition, and motion control. It then discusses challenges with localization using odometry alone due to sensor and actuation uncertainties. The Kalman filter is introduced as a way to estimate the true state by combining predictions with measurements. The summary discusses: 1) Localization is determining a robot's position and is challenging due to sensor and wheel inaccuracies over time. 2) The Kalman filter fuses predictions with measurements to estimate the true state, reducing errors over time. 3) It works in two stages - prediction using a model and correction by updating the prediction based

Uploaded by

Khaled Zaki
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 19

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

You might also like