CIR07-KF_TwoSlides.pdf

7/27/2019 CIR07-KF_TwoSlides.pdf

1/27

Control of Industrial Robot

Cristian Secchi Pag. 1

Ing. Cristian Secchi

Tel. 0522 522235

e-mail: [email protected]

h=p://www.dismi.unimore.it/index.php?qsecchi, h=p://www.arscontrol.org

CONTROL OF INDUSTRIAL ROBOTSLaurea Magistrale in Ingegneria Meccatronica

CONTROL OF INDUSTRIAL ROBOTSTHE KALMAN FILTER

Kalman Filter-- 2

Technology and Uncertainty

In practice, the output of a system is a measurement given byone or more sensors. It is usually affected by a noise that can bedue to several factors: disturbances, construction limits,information quantization,

Actuators (especially the low cost ones) can be inaccurate andthis inaccuracy can be described as a noise on the input.

Models can be inaccurate and their imperfection can bemodeled as a process noise.

Cristian Secchi

All these noises produce both a measurement uncertainty and aprocess uncertainty.


2/27



Kalman Filter-- 3


Cristian Secchi

km

x1x2

u

y

b

process uncertainty: static friction, dissipation of the spring,

quantization of the control action

measure uncertainty: quantization of the measurement (e.g.encoder), noise

Kalman Filter-- 4


Uncertainty is problematic for the estimation of the state. Infact, the noise on the output falsify the state information andthe noise on the process falsify the model used for theobserver.

If the model is accurate enough and sensors and actuatorsare reliable enough, uncertainty can be neglected and thestate observer can be exploited safely.

If the uncertainty becomes significant (e.g.: for low costapplication or for incremental measurements) itis necessaryto consider it in the state estimation problem. In fact themeasurement error (es.: ) is integrated by the model ofthe system and, consequently, noise is integrated and theuncertainty on the state increases.

Cristian Secchi

xCy


3/27



Kalman Filter-- 5

Example

Cristian Secchi

Kalman Filter-- 6

Example

Cristian Secchi

If a noise with average 0.1 and variance 0.5 is present on the processand on the output, the estimate becomes:

y(t)

The estimategets worse!


4/27



Kalman Filter-- 7

Example: Localization

Cristian Secchi

Modeling the uncertaintyand exploiting knowninformation, it is possible tomaster the uncertaintyand to obtain much lessuncertain information

Kalman Filter-- 8

The Kalman filter

It exploits a model of the uncertainty of the measure and ofthe process.

It allows to build a state observer that provides the bestestimate of the state by balancing the information extractedby the model and the information extracted by themeasurement on the basis of reliability considerations. It isused in many many disciplines: control theory, robotics,economy, computer vision,

R.E. Kalman, A New Approach to Linear Filtering andPrediction Problems, Transactions of the ASME 82, 1960

Cristian Secchi

Rudolph Emil Kalman received theNational Medal of Science by BarackObama in 2010


5/27



Kalman Filter-- 9

The Kalman Filter

The noise on the measurements and the uncertainty of themodeled can be described as a stochastic process, namelyas a random variable characterized by some, possibly varying,parameters (mean, variance, )

The characteristics of the uncertainties acting on the system iscrucial for developing the tools for the state observation.

The Kalman filter considers uncertainties that can be modeledby Gaussian random variables. This model fits with many (but

not all!) of the uncertainties present in practical applications.

Cristian Secchi

Kalman Filter-- 10

Probability 101

Let S be the set of all the possible results of a randomphenomenon

S={head, tail} S={1,2,3,4,5,6} S={1u,2u, }

An eventA is a subset of the possible results ( ) The probability that an event A is verified is given by:

Cristian Secchi

A S

Pr(A) =number of the results in A

number of the results in S


6/27



Kalman Filter-- 11

Probability 101

If you throw a regular dice with 6 faces, what is the probabilityto get 1 or 2?

Cristian Secchi

}2,1{=A

}6,5,4,3,2,1{=S

3

1

6

2)Pr( ==A

Kalman Filter-- 12

Probability Axioms

Cristian Secchi

Let A and B two events:

q

q

q

1)Pr(0 A

1)Pr( =S

)Pr()Pr()Pr()Pr( BABABA +=

0)Pr( =

BAA B

True

)Pr(1))(Pr( AAnot =

If A and B are disjointevents, then

)Pr()Pr()Pr( BABA +=


7/27



Kalman Filter-- 13

Discrete Random Variables

Cristian Secchi

A discrete random variable is a variable that can assume arandom value on a set of admissible values. Each admissiblevalue is characterized by a probability to be assumed by therandom variable. Formally, if X is a random variable:

1. X can assume one of a finite (or countable) number of values{x1, x2, , xn}.

2. P(X=xi), or P(xi), is the probability that the random variable Xassumes value xi.

Example: The experiment throw a dice can be represented as adiscrete random variable X that can take value on

{x1, x2, , x6} ={1,2,3,4,5,6} and where P(xi)=1/6 for i=1,,6

Kalman Filter-- 14

Continuous Random Variables

Cristian Secchi

A continuous random variable is a variable that can assume arandom value on a continuous set of admissible values. Eachadmissible value is characterized by a probability to be assumedby the random variable. Formally, if X is a continuous randomvariable:

1) X can assume values in a continuous interval [x1,x2].

2) p(X=x), or p(x) (or f(x)), is the probability density function (pdf)

that the random variable X assumes value x

=b

a

dxxpbax )()),(Pr(

x

p(x)It is more likelythat the variableassumes a value xcorresponding toa p(x) close to thecenter of the bell.


8/27



Kalman Filter-- 15

The experiment throw a dart can bemodeled by a continuous random variable

The continuous set is the target The pdf depends on how good is the player.

For a good player it can be modeled as:

The better the player, the tighter the bell.

Example Darts

Cristian Secchi

x

p(x)

center of the target

Kalman Filter-- 16

The probability sums to 1

Cristian Secchi

=x

xP 1)(

Discrete random variables

=1)( dxxpContinuous random variables

The values that can be assumed by a random variable can beinterpreted as disjoint events

The probability that a random variable assumes a value or another isgiven by the sum of the probabilities of the considered values.

The probability that a random variable assumes one of the possiblevalue is 1.


9/27



Kalman Filter-- 17

Joint and Conditional Probability

Consider two events A and B. Thejoint probability of A and B isthe probability that the two events are verified at the sametime.

Consider two random variables X and Y. The joint probabilitythat X assumes the avlue x and that Y assumes the value y isindicated by P(X=x eY=y) = P(x,y)

If X and Y are independent then P(x,y) = P(x) P(y) The throws of a dice are independent. Each throw can be

modeled by a random variable. The probability of obtaining

twice the same value is 1/6*1/6=1/36

P(x|y) indicates the probability that X=x when Y=y. In otherwords it is the probability of x given y

P(x|y) = P(x,y) / P(y)

P(x,y) = P(x | y) P(y)

If X and Y are independent thenP(x|y) = P(x)

Cristian Secchi

Kalman Filter-- 18

Example

The probability that a player extracts one ace after that theother has extracted one ace is given by:

Cristian Secchi

Consider a card deck. The random phenomenonextraction of the next card can be modeled asa random variable. Two players extract a cardone after the other. The two extractions can bemodeled by two random variables X and Y.

204

12

52

451

3

52

4

)1|1( ==== YXP


10/27



Kalman Filter-- 19

Expected Value

It generalizes the concept of mean value of a random phenomenon. If the phenomenon is modeled as a discrete random variable X, thenthe expected value is:

E[X] represents assumed by X in average considering the probabilitywith which the admissible values can be assumed by X. It is a

weighted average.

Example: X can take values in {x1,x2}={1,2} with P(x1)=0.1 P(x2)=0.9. Theexpected value of X is not the average of the admissible values (=1.5)

but E[X]=0.1*1+0.9*2=1.9. E[X] is closer to 2 because the probability ofassuming the value 2 is much greater than the probability ofassuming the value 1. E[X] is the average of the values of X after asufficiently big number of experiments.

Cristian Secchi

=

=

n

i

iixPxXE

1

)(][

Kalman Filter-- 20

Expected Value

In case of continuous random variables, the definition ofexpected value is conceptually the same.

If the random variable X can take values on R and p(x) is thepdf, then;

Cristian Secchi

= dxxxpXE )(][


11/27



Kalman Filter-- 21

Variance

The variance 2 of a random variable around its expectedvalue is given by:

The variance represents the dispersion of a random variablearound its expected value. The bigger the variance, thebigger is the dispersion around the expected value.

Cristian Secchi

2= E[(XE[X])2 ]= (x E[X])2p(x)dx

=

==n

i

iixPXExXEXE

1

222 )(])[(]))([( Discrete Time

Continuous Time

E[X]=1

2 =1.41

E[X]=1

2 =0.32

Kalman Filter-- 22

Uncorrelated Random Variables

Two random variables X and Y are uncorrelated if

Intuitively, this implies that there is no correlation between tworandom phenomena. A combination between the randomvariables does not introduce extra terms to their expected value

Two independent variables are always uncorrelated but theconverse is not always true.

Cristian Secchi

][][][ YEXEXYE =


12/27



Kalman Filter-- 23

Random Vectors

A random vectorX =(x1,,xn)T is a vector whose componentsare random variables.

The expected value of a random vector is given by:

The Covariance Matrix S of a random vector X is given by

Cristian Secchi

=

][

][

][

1

nxE

xE

XE

]])[])([[( TXEXXEXE =

It is the generalization of the concept of variance!

Kalman Filter-- 24

Random Vectors

In the scalar case, the definition of covariance matrixcoincides with that of variance

The terms on the diagonal are the variances of thecomponents of the random vector X

The terms out of the diagonal indicates the correlationbetween the components of the random vector

If a random vector X is made up of uncorrelated randomvariables, then the covariance matrix is diagonal

Cristian Secchi


13/27



Kalman Filter-- 25

Gaussian Random Variables

A continuous random variable is called gaussian variable (ornormalvariable orsimply gaussian) if its probability density function is a Gauss

curve described by:

It can be proven that: E[X]= E[(X-)2]=2

The expected value is called the average value or the average of thegaussian

The tighter the bell, the smaller the variance A gaussian variable is completely described by its mean and by its

variance

The probability that a gaussian variable assumes a value in [-,+] is 95%Cristian Secchi

2

2

2

)(

2

1)(

=

x

exf

f(x)

x- +

Kalman Filter-- 26

Variabili Aleatorie Gaussiane

A random vector is called gaussian (or normal) it itscovariance matrix is given by

It can be shown that a gaussian random vector assumes avalue that is in the uncertainty ellipsoid with centeranddescribed by (x-)T-1(x-)=1 with a probability of 95%.

It can be shown that: E[X]= E[(X-) (X-)T]=

Cristian Secchi

)()(2

1 1

)det()2(

1)(

=

xx

n

T

exf


14/27



Kalman Filter-- 27

A random variable is associated to a random phenomenon(e.g. temperature in Reggio Emilia during summer) In order to model the random variable (i.e. to define its

characteristic quantities) it is necessary to make severalexperiments and to collect samples (e.g. temperaturemeasurements in Reggio Emilia during summer)

Given a collection of samples (the more the better, at leastmore than 40) there are several normality tests:

Q-Q plot K-squared test

If the variable is gaussian then the mean and the variance canbe computed using the maximum likelihood method as:

where xi are the samples and n is the total number of samples

When is a random variable Gaussian?

Cristian Secchi

=1

n

nX

i=1

xi 2 =

1

n

nX

i=1

(xi )2

Kalman Filter-- 28

Collect samples through experiments Assume that the random variable is normal Compute and (maximum likelihood method) If most (at least more than 60%) of the samples are

concentrated in then the normality assumption isadmissible

A simple empirical normality test

Cristian Secchi

2

[ , + ]

= 1.01 = 0.

55

71 % of the samples falls in [ , + ]

The random phenomenon can beapproximately described as a Gaussianvariable


15/27



Kalman Filter-- 29

Gaussian Random Variables and Robotics

Gaussian Variables are simple because they are describedonly by two parameters. They allow to model uncertainties that are very common in

fixed and, especially, in mobile robotics

Where am I? More or less in this position

The set of gaussian random variables with mean andvariance is usually indicated as N(,) and the set ofgaussian random vector with mean and covariance matrixis indicated with N(, )

Cristian Secchi

Kalman Filter-- 30

The Kalman Filter

The Kalman filter will be illustrated for discrete time systems The filter is used when it is necessary to consider the nois of the

process and/or of the measurements and, therefore, the discrete

ouput of the sensors.

Its extended version is very used in mobile robotics The continuous version of the filter is similar

Cristian Secchi


16/27



Kalman Filter-- 31

Discrete Time Kalman filter

It allows to estimate the state x(k) of a discrete LTI systemdescribed by:

w(k) is a random vector representing the process uncertainties v(k) is a random vector representing the measurement

uncertainties

Cristian Secchi

+=

++=

)()()(

)()1()1()(

kvkCxky

kwkBukAxkx

Kalman Filter-- 32

Kalman Filter - Assumptions

w(k) is a gaussian vector with zero mean and knowncovariance matrix Q(k)

v(k) is a gaussian vector with zero mean and knowncovariance matrix R(k)

w(k) and v(k) are independent variables the uncertainty onthe measure does not depend on the uncertainty on theprocess and viceversa

w(k) and v(k) encode pure noise that contains all the randomuncertainties of the system.

Matrices Q(k) and R(k) can be obtained by using themaximum likelihood test on the components of w(k) and ofv(k). Even with approximate initial estimates, the filter usuallygives good results.

Cristian Secchi


17/27



Kalman Filter-- 33

Kalman Filter

The initial state of the system x(0) is a gaussian random vector with amean 0 and a covariance matrix 0. The state x(k) and the output y(k) are gaussian random vectors since

they are a linear combination of gaussian random vectors.

The main idea of the Kalman filter is to exploit the knowledge of themodel and the statistical description of the uncertainties to decidehow to combine in an optimal way the information about the statecoming from the model and the one coming from the measurements

The output of the filter is a gaussian random vector representing astatistical estimate of the state of the system. The mean of theestimate represents the expected value of the state and thecovariance matrix represents the uncertainty of the expected value.

Cristian Secchi

Kalman Filter-- 34

Kalman Filter the algorithm

It is recursive Both the estimation at time k-1 and the measurement at

time k are necessary for computing the estimate at time k

It allows to have at time k the estimate of the state x(k). Thisis usually sufficient fro planning the motion of a robot or forcontrolling the system.

The estimate is given by two terms: : The expected value of the estimate (k): the covariance matrix of (an indicator of the

accuracy of the staet estimate)

The filter needs to be initialized and then it loops between twostates

Prediction Update

Cristian Secchi

)( kx

)( kx


18/27



Kalman Filter-- 35

Initialization

It is necessary to initialized the state of the filter with a gaussianrandom vector

As for the deterministic state observers, the closer the initialstate of the filter to the real state, the faster the convergenceof the estimate to the real state of the system will be.

If an approximate estimation 0 or the initial state of thesystem is available, the kalman filter should be initialized withsuch estimate and with an adequate covariance matrix.

If no estimate of the initial state of the system is available, theKalman filter can be initialized with a random vector and a bigcovariance matrix in order to indicate that the initial estimateis inaccurate.

Cristian Secchi

Kalman Filter-- 36

The algorithm (Notation 1)

Cristian Secchi

PredictionPredicted statePredicted covariance

Update InnovationCovariance of the innovationKalman gainState updatedCovariance update

)1()1|1()1|( += kBukkxAkkx

)()1|1()1|( kQAkkAkkT+=

)1|()()( = kkxCkyke

)()1|()( kRCkkCkS T +=

1)()1|()( = kSCkkkKT

)()()1|()|( kekKkkxkkx +=

)1|())(()|( = kkCkKIkk


19/27



Kalman Filter-- 37

The algorithm (Notation 2)

Cristian Secchi

)1()1()( += kBukxAkx

)()1()( kQAkAk T +=

= )()()( kxCkyke

)()()( kRCkCkS T +=

1

)()()(

= kSCkkK

T

)()()()( kekKkxkx +=

= )())(()( kCkKIk

PredictionPredicted statePredicted covariance

Update InnovationCovariance of the innovation

Kalman gain

State updatedCovariance update

Kalman Filter-- 38

The Kalman filter

Cristian Secchi

(1) State prediction(2) Covariance prediction )1()1()( +=

kBukxAkx

)()1()( kQAkAk T +=

Prediction

(1) Compute Kalman gain(2) Update the estimate(3) Update covariance

1))(()()( += RCkCCkkK TT

)()()()( kekKkxkx +=

= )())(()( kCkKIk

Update

)1()1( kkx


20/27



Kalman Filter-- 39

Example

Cristian Secchi

+=

++=

)()(4)(

)()1(2)1(3)(

kvkxky

kwkukxkx

C

BA

8.0

1.0

=

=

R

Q

78.43)(1.0)1(1)1(3)1( ==== kykkukx

11)1()1()( =+= kBukxAkx

1)()1()( =+= kQAkAkT

22.0)()()( ==kxCkyke

8.16)()()( =+=

kRCkCkS

T

23.0)()()( 1 ==

kSCkkKT

94.10)()()()( =+= kekKkxkx

08.0)())(()( ==

kCkKIk

prediction

The covariance get worse because of the process uncertainty

Build the innovation using the output

The innovation is very uncertain

The Kalman gain is small in order to avoid to weight theinnovation too much and to introduce a big uncertainty.

The correction to the predicted state is small

The obtained estimate has a smaller uncertainty thanks to thecorrection done by the innovation.

Kalman Filter-- 40

Kalman Filter - Illustration

Cristian Secchi

A robot is moving along the x-axis. The robot knows a model of itselfand it receives information on its position by a sensor (GPS, camera,). Both the model and the sensor are not precise and, therefore,both the state and the measure can be represented as gaussianrandom variables.


21/27



Kalman Filter-- 41


Cristian Secchi

The position of the robot is a gaussianrandom variable. The robot knowswhere it is (mean value peak of thebell) with some uncertainty(variancewidth of the bell)

The robot reads the output of thesensor in order to improve itsknowledge about its position. Itreceives a measurement which sayswhere it is with some uncertainty.

The Kalman filter merges theinformation of the position of therobot with that coming from thesensor and it gets a more preciseestimate of its position.

Kalman Filter-- 42


Cristian Secchi

If the robot moves to the right, it gets fromthe model (prediction) an estimate of itsposition which is unprecise because ofthe uncertainty of the model.

The robot reads the sensor which providesan uncertain measure of the position ofthe robot.

Integrating the estimate of the modeland the measure using the fKalman filte anew, better estimate of the position ofthe robot is obtained.


22/27



Kalman Filter-- 43

Remarks

The Kalman filter provides the best estimate of the state incase the uncertainties can be modeled as gaussians. It updates the estimate by a weighted average on the

uncertainties.

If the process covariance is high, then the Kalman gainweights more the innovation.

If the covariance of the innovation is high, the Kalman gainweights more the prediction

In the continuous time case, the Kalman gain is obtained in asimilar way and it can be used as a gain in a state observer forobtaining an optimal estimate of the state in case gaussianuncertainties are acting on the process and on themeasurements.

Cristian Secchi

Kalman Filter-- 44

Example

Cristian Secchi

A voltage generator produces an unknown constant value. Theproduced voltage can be measured but the measurement iscorrupted by a noise with variance 0.1 V. Estimate the producedsignal using the Kalman filter


23/27



Kalman Filter-- 45

Example

Cristian Secchi

The process can be modeled as a LTI system with an uncertainty

x(k) = Ax(k1)+ Bu(k1)

x(k) = x(k1)01)( == BARkx

The state does not changeand there is no input

The output is characterized by an uncertainty

)()()( kwkCxky +=

1=C)()()( kwkxky +=

The full state is measured

w(k) is a gaussian random variable with zero mean and variance0.1

Kalman Filter-- 46

Example

Cristian Secchi


24/27



Kalman Filter-- 47

Example

Cristian Secchi

Considera fully actuate holonomic robot moving on a plane

x

y

ux

uy

The position of the robot on the plane is detected by a GPS sensorcharacterized by some uncertainty.

Kalman Filter-- 48

Example

Cristian Secchi

The robot can be modeled as a discrete LTI system.

+=

++=

)()()(

)()1()1()(

kvkxky

kwkukxkx

=

y

xx

=

y

x

u

uu

))(,0()( kQNkw ))(,0()( kRNkv

==

05.00

005.0)( QkQ

==

20

02)( RkR

Small uncertainty

Bigger uncertainty


25/27



Kalman Filter-- 49

Observation

The robot starts from x=0 and y=1 and it receives as inputsux(k)=sin(0.01k) uy(k)=cos(0.1k)]

Cristian Secchi

The uncertainty corrupts the measurement and from the output itis not possible to accurately understand the motion of the robot.

)(ky )(ky and real trajectory

Kalman Filter-- 50

Example

Using the Kalman filter it is possible to exploit the knowledge ofthe model for building a more accurate estimate of the state.

The estimate is initialized with:

Cristian Secchi

[ ]10)0( =x

=

40

04)0(

)( kx e traiettoria reale)( kx


26/27



Kalman Filter-- 51

Example

Cristian Secchi

The filter starts form a very inaccurate estimate and the estimatebecomes more and more accurate thanks to the fusion of the dataof the model with the measure.

Kalman Filter-- 52

Example

The system starts from and initia state [10;10] and the controlinput is given by:

The designed input makes the closed loop systemasymptotically stable and makes the robot to go to 0.

Cristian Secchi

)(5.00

05.0)( kxku

=


27/27


Kalman Filter-- 53

Example

Cristian Secchi

)()( kKyku = )()( kxKku =

The noise on the output makes the performance of the controllerworse. Using the Kalman filter performance improves considerably.

Ing. Cristian Secchi

Tel. 0522 522235

e-mail: [email protected]

h=p://www.dismi.unimore.it/index.php?qsecchi, h=p://www.arscontrol.org

CONTROL OF INDUSTRIAL ROBOTSLaurea Magistrale in Ingegneria Meccatronica

CONTROL OF INDUSTRIAL ROBOTS

THE KALMAN FILTER

CIR07-KF_TwoSlides.pdf

Documents

Transcript of CIR07-KF_TwoSlides.pdf