State Estimation Classical Themes of Computer Science …

Gerald Steinbauer

Institute for Software Technology

1

State Estimation - Homework

State EstimationClassical Themes of Computer Science

Homework

Gerald SteinbauerInstitute for Software Technology

Gerald Steinbauer


2


Agenda

• Organizational Issues• Short Introduction to Matlab• Homework Assignment • Live Demo

Gerald Steinbauer


3


Organizational Issues

• Dates– 14.01.2009 11:45-14:00 (HS i11) introduction homework– 16.01.2009 23:59 group announcement per email

[email protected]– 21.01.2009 18:00-18:45 (HS i11) question hour– 28.01.2009 23:59 submission of code per email

[email protected]– 29.01.2009 & 30.01.2009 9:00-13:00 group interviews

• Homework– implementation of a Extended Kalman Filter for a Robot Localization

Problem– using Matlab– groups of up to five students (if possible at least one member with

Matlab experience)

Gerald Steinbauer


4


Access to Matlab

• you are free to use your private Matlab• Matlab is further installed on all computers in the

“Subzentren”• reserved time in “Subzentrum Neue Technik”

– Room NT03050 - (EDV-Lernzentrum Kopernikusgasse, CAD-Raum)

– Kopernikusgasse 24, 3.Obergeschoß– 7.-23.2009 (Mo.-Fr.) 8:00 - 12:00 each and 28.01.2009, 8:00 -

18:00

• on-line tutorial http://www.math.ufl.edu/help/matlab-tutorial/

Gerald Steinbauer


5


Matlab

• commercial mathematic toolbox• interactive programming environment• interpreter for scripts• a huge number of build-in functions• computation is based on matrices• excellent and quick visualization functions• a lot of toolboxes and software available• popular tool in engineering and science

Gerald Steinbauer


6


Matrices

• Computation is bases on Matrices • NxM matrix has N rows and M columes• M=[1 2 3 ; 4 5 6] (‘;’ separates rows)

• size(M) provides the dimension of the matrix M

• return value are also matrices• matrices can be assigned to variables

⎟⎟⎠

⎞⎜⎜⎝

⎛=

654321

M

( )32

Gerald Steinbauer


7


Indexing

• Matlab allows access to parts of a matrix• M=[1 2 3; 4 5 6;7 8 9]

• M(:,1) provides all values of the first column

• M(2,2:3) provides all value 2-3 of the second column

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

987654321

M

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

741

( )54

Gerald Steinbauer


8


Branching

• Matlab supports a number of relations and operators– a==b, a!=b, a>b, ….– ~c, c&d, c|d

• if statementif condition command end

if condition command1 else command2 end

• if statements can be nested

• an ‘;’ at the end of a statement hides its output in the command line

Gerald Steinbauer


9


Loops

• for loopif loopcommand command end

• Matlab supports differed loopcommands– i=a:b (i starts at a and is incremented by 1 until it is equal b)– i=a:c:b (i starts at a and is incremented by c until it is equal b)– i=[1 4 5 8 2 5] (i takes successively the numbers in the matrix)

• while loopwhile condition command end

• loop statements can be nested

Gerald Steinbauer


10


Functions

• Matlab has a number of build-in standard functions– trigonometric, matrix inversion, random numbers, infix operators, …

• one can write new functions– have to be implemented in a file called “function_name.m”– signature function [rv1 rv 2 ...] = function_name(p1,

p2, ...)

• rv1 rv2 … are the return values of the function (can be omitted)• p1, p2, … are the parameters of the function (can be omitted)

function [s d] = sum_diff(a,b)

s=a+b

d=a-b

Gerald Steinbauer


11


Visualization

• Matlab has a number of excellent build in visualization functions

• plot[3](m_x, m_y,…) makes a 2d or 3d plot– m_x values on x axis (nx1)– m_y values on y axis (nx1)– m_z values on y axis (nx1)– can be parameterized in color and appearance

• surf(m_x, m_y, m_z…) makes a 3d surface plot– m_x values on x axis (nx1)– m_y values on y axis (mx1)– m_z values on y axis (nxm)– can be parameterized in color and appearance

Gerald Steinbauer


12


a=0:0.1:2*pib=sin(a)plot(a,b)

a=0:0.1:2*pib=sin(a)c=cos(a)d=b’*cplot3d(a,a,d)

Gerald Steinbauer


13


Little Helper

• variable ans contains always the result of the last command

• who reports all currently defined variables• whos reports all currently defined variables including

their size• a’ transposes a matrix• randn(‘seed’,some_number) restarts the random

number generator

Gerald Steinbauer


14


On-Line Help

• “help function-name” in the command window provides quick additional information on all build-in functions

• Matlab has a build-in help browser including search function

Gerald Steinbauer


15


Homework

• implement an Extended Kalman Filter (EKF) for a landmark-based localization for a RoboCup four-legged Robot (see slides of lecture)

• derive all necessary parts of the EKF– representation of the state– prediction (control)– update (measurement)

• visualize the results of the state estimation

• investigate the influence of different noise

Gerald Steinbauer


16


Live DemoMatlab

Gerald Steinbauer


17


Landmark-based Localization

z1 z2

z3X

Y

Θ

State: x=[X,Y,Θ]’ Observation: zi=[φi,si]’Control: u=[v,ω]’

v

ω

Gerald Steinbauer


18


Matlab Simulation Package

• it generates motion-commands for the movement of a robot on a RoboCup four-legged Robot

• it simulates the uncertain movement of the robot• it simulates the noise observations of the landmarks

on the field

• state of the robot [x,y,Θ]• control (odometry based) [δ1,δtrans,δ2]• observation (bearing and id of landmark) [φ,c]

• can be downloaded from website of the lectureThanks to Prof. Dieter Fox

Gerald Steinbauer


19


What to implement

• run.m - main update loop, should call ekfUpdate– call it: run(number_of_simulation_steps)– integrate the EKF update and the visualization

• ekfUpdate.m - EKF update – implement all steps of the EKF– adapt the signature according to your needs

Gerald Steinbauer


20


Functions in the Package (Simulation)

• generateScript.m - generates data according to initial mean and noise parameters

• generateMotion.m - simulates simple motion commands

• prediction.m - move robot according to specified motion

• sampleOdometry.m - samples a movement according to the model and the noise

• sample.m - samples from a covariance matrix • getfieldinfo.m - gets field information (the 6

landmarks)

Gerald Steinbauer


21


Functions in the Package (Visulaization)

• plotcircle.m - draws a circle• plotcov2d.m - draws a 2-D covariance matrix• plotfield.m - draws the field with landmarks• plotmarker.m - draws an 'x' at a specified point• plotrobot.m - draws the robot

Gerald Steinbauer


22


Running Example (no EKF)

landmarks

robot

commanded path

true path

noisy observationof landmark 1

Gerald Steinbauer


23


Live DemoLocalization

Documents

State Estimation Classical Themes of Computer Science …