state estimate

introductiondeterministic observer

stochastic observer

A short introduction to

State Estimate

Gianluca Antonelli

Universita degli Studi di Cassino e del Lazio [email protected]

http://webuser.unicas.it/lai/robotica

http://www.eng.docente.unicas.it/gianluca antonelli

Gianluca Antonelli Evry, 27-29 march 2013

mailto:[email protected]

http://webuser.unicas.it/lai/robotica

http://www.eng.docente.unicas.it/gianluca_antonelli


stochastic observer

Outline

Introduction

A naive (i.e., open loop) solution

The deterministic case (Luenberger)

The stochastic case (Kalman)



stochastic observer

Applications

Estimate of vel/acc measuring the position

Estimate of pos/acc measuring the velocity

Inertial Navigation Systems

Feature traking by vision systems

Localization

Mapping

Simoultaneous localization and mapping

Estimate of the battery charging

Econometry

Tracking by radar

Missile pointing

Insuline-glucose control

Global Positioning System

. . .



stochastic observer

Modeling

We will deal with ISO (Input State Output) dynamic system describedby differential or difference equations

continuos time

x(t) = f(x(t),u(t), t)y(t) = h(x(t), t)

discrete time

x(k + 1) = f(x(k),u(k), k)

y(k) = h(x(k), k)

x ∈ Rn,u ∈ R

p,y ∈ Rm

assuming linearity and time invariance

c.t.

x(t) = Ax(t) +Bu(t)y(t) = Cx(t) +Du(t)

d.t.

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

y(k) = Cx(k) +Du(k)



stochastic observer

Introduction

Measurements give an incomplete view of the stateThe filter is required to estimate the state based on the modelknowledge

state measures filter



stochastic observer

Introduction

Is it possible to estimate the state of a dynamic system without directmeasurement of it?

u y

x ?

Σ



stochastic observer

Open loop observer I

We build a copy of the system, feeded in parallel with the same input

uy

x

x

Σ

Σ



stochastic observer

Open loop observer II

Given an ISO system

c.t.


d.t.

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

y(k) = Cx(k) +Du(k)

x ∈ Rn,u ∈ R

p,y ∈ Rm

defining x(·) the x(·) estimate, we build

c.t.

˙x(t) = Ax(t) +Bu(t)

d.t.

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

We define the error



stochastic observer

Open loop observer III

c.t.

e(t) = x(t)− x(t)

d.t.

e(k) = x(k)− x(k)

with dynamics:

c.t.

e(t) = x(t)− ˙x(t)= Ax(t)−Ax(t)= Ae(t)

d.t.

e(k + 1) = x(k + 1)− x(k + 1)= Ax(k)−Ax(k)= Ae(k)

and thus evolution:

c.t.

e(t) = eAte(0)

d.t.

e(k) = Ake(0)



stochastic observer

Open loop observer IV

which is unsuitable for several reasons

The estimate dynamics is related to the eigenvalues of A and itcan not be modified

It is necessary to assume Σ asymptotically stable

Knowledge of the output (a linear combination of the state) is notexploited

The system is known with an identification error, only Σ isavailable



stochastic observer

problem formulationsolutionexercises

Closed loop observer (Luenberger)

It is required to estimate the state by exploiting both the input andthe output

u y

x

x

Σ

obs



stochastic observer


Problem formulation c.t. and d.t. I

Given and ISO system

c.t.


d.t.

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

y(k) = Cx(k) +Du(k)

x ∈ Rn,u ∈ R

p,y ∈ Rm

and the observer structure

c.t.

˙x(t) = F x(t) + Γu(t) +Gy(t)

d.t.

x(k+1) = F x(k)+Γu(k)+Gy(k)



stochastic observer


Problem formulation c.t. and d.t. II

it is necessary to design F , Γ and G such that:

c.t.

limt→∞

‖x(t)− x(t)‖ = 0

∀u(t),∀x(0),∀x(0)

d.t.

limk→∞

‖x(k)− x(k)‖ = 0

∀u(k),∀x(0),∀x(0)

i.e., that the estimate converge to the true value for all the initialconditions of both the system and the observer and for all the possibleinputs



stochastic observer


Solution c.t. and d.t. I

Defining the error

c.t.

e(t) = x(t)− x(t)

d.t.

e(k) = x(k)− x(k)

with dynamics:

c.t.

e(t) = x(t)− ˙x(t)= Ax(t) +Bu(t)+

−Fx(t)− Γu(t)−Gy(t)

d.t.

e(k + 1)=x(k + 1)− x(k + 1)= Ax(k) +Bu(k)+

−F x(k)− Γu(k)−Gy(k)

to make it independent from the input we impose Γ = B



stochastic observer


Solution c.t. and d.t. II

assuming D = O and substituting y = Cx, it holds

c.t.

e(t) = (A−GC)x(t)− F x(t)

d.t.

e(k+1) = (A−GC)x(k)−F x(k)

F is designed so that an homogeneous system holds:

F = A−GC

in this way the error dynamics is governed by the equation:

c.t.

e(t) = Fe(t)

d.t.

e(k + 1) = Fe(k)



stochastic observer


Solution c.t. and d.t. III

and thus the evolution:

c.t.

e(t) = eF te(0)

d.t.

e(k) = F ke(0)

It is thus necessary that the eigenvalues of the dynamic matrix F allstay in the stability region so that the error converges to zeroWe need to design G and F (why it is not possible G = O and F = A?)We design the n desired eigenvalues λd,i of F :

λd = λd,1, . . . , λd,n

those give the coefficients of the desired characteristic polynomium:

n∏

i=1

(λ− λd,i) = λn + d1λn−1 + . . .+ dn



stochastic observer


Solution c.t. and d.t. IV

The characteristic polynomium

|λI − (A−GC)|

is thus fixedThe unknowns are the elements of the matrix G (why it is not possible

to solve a system by resorting to the identity principle of polynomia?)Let us consider a scalar output, matrices C and G, thus, are row orcolumn vectors c and g

We compute the characteristic polynomium of A

|λI −A| = λn + a1λn−1 + . . .+ an



stochastic observer


Solution c.t. and d.t. V

We compute the matrix:

T =[

cT ATcT . . . ATn−1cT

]

an−1 an−2 . . . a1 1an−2 an−3 . . . 1 0. . . . . . . . . . . . . . .. . . . . . . . . . . . . . .1 0 . . . 0 0

that is full rank if the first matrix is, i.e., OT (the transpose of theobservability matrix)By defining

d =[dn . . . d1

]T

a =[an . . . a1

]T



stochastic observer


Solution c.t. and d.t. VI

it is possible to write the Ackermann formula:

g = −T -T (a− d)

This gives univocally the unknown g, and thus the matrix F



stochastic observer


Selection of the F eigenvalues

The observer needs to converge as fast as possible to the true valueTheoretically the eigenvalues may be placed everywhere in the complexplaneIn practice, a bound is given by the measurements noise

c.t.I

R

λa

λd

d.t.I

R

1

λa

λd



stochastic observer


Model knowledge

Let us assume C known, knowledge of B and A is affected by an error:

F = A−GC = A− A−GC

Γ = B = B − B

the error evolution is thus characterized by:

c.t.

e(t) = Fe(t) + Ax(t) + Bu(t)

d.t.

e(k+1) = Fe(k)+Ax(k)+Bu(k)

that is not anymore homogeneous(A may exhibits unstable eigenvalues, it is an issue?)



stochastic observer


Reduced observer I

If the system is not observable it is possible to implement a reducedobserver that gives and estimate of the sole observable componentBy resorting to a proper matrix of equivalence the system istransformed into the canonical form

c.t.

z(t)=

[

Ao O

A1 Ano

]

z(t) + P−1Bu(t)

y(t)=[

Co O]

z(t)

d.t.

z(k + 1)=

[

Ao O

A1 Ano

]

z(k) +P−1Bu(k)

y(k)=[

Co O]

z(k)



stochastic observer


Reduced observer II

the observable dynamics, to be used to design the observer, is:

c.t.

zo(t) = Aozo(t) + P−1B(1 : o, :)u(t)y(t) = Cozo(t)

d.t.

zo(k + 1)=Aozo(k) + P−1B(1 : o, :)u(k)

y(k) =Cozo(k)

of dimension o < nIt is meaningless to come back into the original state x



stochastic observer


A feedback interpretation

The observer may be rewritten as

c.t.

˙x(t) = Ax(t) +Bu(t) +G (y(t) −Cx(t))

d.t.

x(k+1) = Ax(k)+Bu(k)+G (y(k)−Cx(k))

thus

c.t.

˙x(t) = Ax(t) +Bu(t)︸︷︷︸

emulation

+G (y(t) − y(t))︸︷︷︸

feedback

d.t.

x(k+1) = Ax(k) +Bu(k)︸︷︷︸

emulation

+G (y(k)− y(k))︸︷︷︸

feedback

where it is possible to appreciate one component that emulate theprocess and a feedback action on the output error, the matrix G playsthe role of a gain



stochastic observer


Operative steps c.t./d.t.

Steps for the design of an asymptotic (Luenberger) observer:

1 impose Γ = B

2 compute the desired eigenvalues by assigning the vector d

3 compute the transformation matrix T

4 compute g

5 compute F

In case of vectorial output m > 1 it is possible to resort to propercommands of numerical software under the description pole placement

(ex: ppol in Scilabor place in Matlab)



stochastic observer


Extended Luenberger filter I

Assuming a nonlinear dynamic system

c.t.

x(t) = f(x(t),u(t), t)y(t) = h(x(t), t)

d.t.

x(k + 1) = f(x(k),u(k), k)

y(k) = h(x(k), k)

by resorting to the closed-loop interpretation it is possible to write

c.t.

˙x(t) = f(x(t),u(t), t)︸︷︷︸

emulation

+G (y(t) − y(t))︸︷︷︸

feedback

d.t.

x(k+1) = f(x(k),u(k), k)︸︷︷︸

emulation

+G (y(k)− y(k))︸︷︷︸

feedback



stochastic observer


Extended Luenberger filter II

where

y(·) = h(x(·), ·)

and the gain, both for the c.t. and the d.t., is computed by resorting tothe same formulas of the linear design where matrices A and C arecomputed by linearization around the current estimate:

C(·) = ∂∂xh(x)

∣∣x=x(·)

A(·) = ∂∂xf(x)

∣∣x=x(·)



stochastic observer


Exercise 1 I

Design a Luenberger observer for the system

A =

0.9210 −0.1497 0.03520.0691 0.6841 0.0123−0.0842 0.2943 0.8525

B =

−2−1−1

C =[0 −3 −1

]

Validate with a numerical simulation

step input with unitary amplitude

build two superblocks, one for the real model and one for theobserver

plot in the oscilloscope both the states and the estimate error



stochastic observer


Exercise 2 I

Design a Luenberger observer for the RLC circuit

R

L

CVi

Vu

iLiC

with data

R = 680Ω

L = 1.5 · 10−3H (Rl = 0.4Ω)

C = 5 · 10−9F


square wave input with amplitude 3Volt e frequency 7KHz

final simulation time tf = 1milliseconds

sampling time T = 0.4microseconds

build two superblocks, one for the real model and one for theobserver



stochastic observer


Exercise 2 II

plot in the oscilloscope both the states and the estimate error

Validate again with a numerical simulation and the real data stored inthe file RLC 02 small.txt

consider the sole observer superblock developed

import the data in the workspace via the commandA=read(’RLC 02 small.txt’,-1,3), the second column stores theinput, the third the output

read input and output of the real model from the workspace woththe block From Workspace

notice that the output needs to be properly transalted to take intoaccount an acquisition error



stochastic observer

problem formulationsolutionexamples and exercises

Definitions on stochastic variables I

Given a stochastic variabile x ∈ Rn with probability density

function fx(x) its expected value (average) is

E[x] = µx =

∫

Rn

xfx(x)dx ∈ Rn

The covariance matrix is defined as

P x = E[(x− µx)(x− µx)

T]

∈ Rn×n



stochastic observer


Definitions on stochastic variables II

A multivariate Gaussian distribution is the function

fx(x) =1

√

(2π)n ‖P x‖e−

12(x−µ

x)TP x(x−µ

x)

In the bidimensional case it exhibits a graphical interpretation

µx

v1v2

average µx

isopotential curves are ellipses

the principal axes of the ellipsesare parallel to the eigenvectorsvi of P x

the square root of a principalaxis is equal to thecorresponding eigevalue



stochastic observer


Definitions on stochastic variables III

x1

x2

µx

v1

v2



stochastic observer


Mathematical formulation

Given a stochastic dynamic system in the discrete time

x(k + 1) = f(x(k),u(k), k) +w(k)

y(k) = h(x(k),u(k), k) + v(k)

wherex ∈ R

n statoy ∈ R

m uscitau ∈ R

p ingressow ∈ R

n rumore di processov ∈ R

m rumore di misura

find an optimal estimation for x(k)



stochastic observer


Linear model

Let us consider first the stationary linear model

x(k + 1) = Ax(k) +Bu(k) +w(k)

y(k) = Cx(k) + v(k)

where

E[w(k)] = 0

E[v(k)] = 0

E[w(i)w(j)T] = Rwδ(i − j) con Rw > O

E[v(i)v(j)T] = Rvδ(i − j) con Rv > O

E[wh(i)vl(j)] = 0 per ogni i, j, h, lE[xh(i)vl(j)] = 0 per ogni i, j, h, l



stochastic observer


Variable definition

x(k) estimate k|kx(k) estimate k|k − 1x(k) error x(k) = x(k)− x(k)P (k) estimate error covariance k|k

P (k) estimate error covariance k|k − 1

k

x(k)

P (k)temporal update

x(k+1)

P (k+1)

k + 1

x(k+1)

P (k+1)



stochastic observer


Kalman filter equations

temporal update(prediction)

measurement update(correction)

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

P (k + 1) = AP (k)AT +Rw

K(k) = P (k)CT[Rv +CP (k)CT

]−1

x(k) = x(k) +K(k) [y(k)−Cx(k)]

P (k) = P (k)−K(k)CP (k)

Update equations not necessarily synchronousPossibility to fuse/merge different measurement sourcesSuitable to be implemented for the non stationary case



stochastic observer


Sketch

x(k) x(k + 1) x(k + 1)

prediction correction

model measurement



stochastic observer


Gain geometric interpretation I

Let us formulate a geometric interpretation of the term

∆x = K(k) [y(k)−Cx(k)]

that appears in the measurement update

x(k) = x(k) +K(k) [y(k)−Cx(k)] = x(k) +∆x

From the output equation we have

y(k) = C (x(k) +∆x) + v(k)

assuming first absence of noise, it allows to write the followingminimization problem

min ‖∆x‖P s.t. C∆x = y(k)−Cx(k)



stochastic observer


Gain geometric interpretation II

where

‖∆x‖P = ∆xTP (k)−1∆x

Notice

the equation y = Cx represents an affine subspace of dimension min the state space

the Mahalanobis norm ‖∆x‖P wheigths more the better estimatedstate components

the problem is underconstrained ⇒ infinite solutions ∆x exists

the state is a stochastic variable with probability density functionof kind Gaussian multivariate



stochastic observer


Gain geometric interpretation III

The minimum norm solution is

∆x = C† (y(k)−Cx(k)) = PCT(CPCT

)−1(y(k)−Cx(k))

By adding measurement noise with covariance Rv one obtains the gainformula

∆x = PCT(CPCT +Rv

)−1(y(k)−Cx(k))



stochastic observer


Gain geometric interpretation

In the numeric case:

x1

x2

x(k)

y(k) = Cx(k)

n = 2m = 1C =

[1 1

]



stochastic observer



without noise and symmetric covariance

∆x

x1

x2

x(k)

y(k) = Cx(k)

n = 2m = 1C =

[1 1

]

P (k) =

[1 00 1

]

Rv = 0

K(k) =

[11

]



stochastic observer



without noise and asymmetric covariance

∆x

x1

x2

x(k)

y(k) = Cx(k)

n = 2m = 1C =

[1 1

]

P (k) =

[1 00 2

]

Rv = 0

K(k) = 13

[12

]



stochastic observer



with noise the subspace is uncertain:

∆x

x1

x2

x(k)

y(k) = Cx(k)

n = 2m = 1C =

[1 1

]

P (k) =

[1 00 2

]

Rv = 1

K(k) = 14

[12

]



stochastic observer


Covariance dynamics

The filter may be interpreted even as propagation of theprediction-correction estimate average value and covariance

x(k)

x(k)x(k)

x(k)

x(k + 1)

x(k + 1) = 0.9x(k) + w(k)y(k) = x(k) + v(k)

with

Rw = rw = 1

Rv = rv = 0.5



stochastic observer


Gain matrix I

An alternative formulation

K(k) = P (k)CTR−1v

where it is possible to notice

Proportional to the error covariance

Inversely proportional to the measurement error covariance

The process noise covariance influences P (k)when Rw = O ⇒ P (k) → O and K(k) → O

The relative wheight between measurement and process noisecovariances represents the relative trust between model andmeasurement

Good model ⇒ small gainGood measurement ⇒ large gain



stochastic observer


Gain matrix II

Under assumptions of

linearity

stazionarity

observability

reachability

K(k) → K∞

with K∞ solution of the Riccati equation

K∞ = P∞CT[Rv +CP∞CT

]−1



stochastic observer


The optimum transient observer

We search the gain K(k) such that:

E[x− x] = 0

P (k) minimum

⇓

The solution is formally equal to the Kalman filter



stochastic observer


Witheness of the innovation

We search the gain K(k) such that:

E[e(i)e(j)T

]= Rvδ(i − j)

with e(k) = y(k) −Cx(k) (innovation)

⇓

The solution is formally equal to the Kalman filterThe hortogonality between estimate and error holds

E[x(k)x(k)T

]= O



stochastic observer


Stability

The state estimate evolves following the dynamics

x(k) = (A−K(k)CA) x(k − 1)

An anlytical proof requires hard constraintsWhen the system is observable E[x(k)] is bounded ∀kStable for most of practical applications



stochastic observer


Example: estimate of a constant I

We have at disposal the noisy measurement of a constant with trivialdynamic

x(k + 1) = x(k) no noise ⇒ (I trust the model!)

y(k) = x(k) + v(k)

where A = 1, H = 1 e E[v(i)v(j)] = rvδ(i− j)The covariance matrix is given P (0) = p(0) = p0



stochastic observer


Example: estimate of a constant II

The update equations of the error covariance and of the gain are:

K(k) =p(k)

rv + p(k)

p(k) =p(k)rv

rv + p(k)p(k + 1) = p(k)

(the scalar gain K is now scalar but we keep it in boldface to differentiate it from

the time)



stochastic observer


Example: estimate of a constant III

At the initial instant we have:

K(0) =p0

rv + p0

p(0) =p0rv

rv + p0p(1) = p(0)

for k = 1 we got:

K(1) =p(1)

rv + p(1)=

p(0)

rv + p(0)=

p0rvr2v + p0rv + p0rv

=p0

rv + 2p0

p(1) =p0rv

rv + 2p0

p(2) =p0rv

rv + 2p0



stochastic observer


Example: estimate of a constant IV

by iteration it is possible to compute symbolically the gain as

K(k) =p0

rv + (k + 1)p0=

p0/rv1 + (k + 1)p0/rv

the estimate is then given by:

x(k) = x(k) +p0/rv

1 + (k + 1)p0/rv[y(k)− x(k)]

where we notice

for increasing k, the new measurements are not used anymore toupdate the estimate

the gain is strongly affected by the ration between p0 and rv

to avoid that the gain tends to zero it is necessary to introduce aprocess noise



stochastic observer


Extended Kalman filter

Idea: we use the nonlinear function whenever possible, otherwise weuse the Jacobians (dynamics linearization):

C(k) = ∂∂xh(x)

∣∣x=x(k)

A(k) = ∂∂xf(x)

∣∣x=x(k)

K(k) = P (k)C(k)T[

Rv + C(k)P (k)C(k)T]−1

x(k) = x(k) +K(k) [y(k)− h(x(k), k)]

P (k) = P (k)−K(k)C(k)P (k)x(k + 1) = f(x(k), k) +Bu(k)

P (k + 1) = A(k)P (k)A(k)T +Rw



stochastic observer


Example: estimate of the model’s parameters I

Possible use: estimate the value of the constant value of the parameterof a physical modelLet us assume θ as the vector of the model unknown parametersThe model is:

x(k + 1) = f(x(k),u(k),θ, k) +w(k)

y(k) = h(x(k),θ, k) + v(k)

Idea: we consider such parameters, constants, as additional states ofthe system with dynamics:

θ(k + 1) = θ(k) +wθ

Let us define the extended state vector:

xE =

[x

θ

]



stochastic observer


Example: estimate of the model’s parameters II

the moidel becomes:

xE(k + 1) = fE(xE(k),u(k), k) +wE(k)y(k) = h(xE(k), k) + v(k)

where

wE =

[w

wθ

]

The noise wθ represents the uncertainty of the parameter’s valueThe filter admits thus its variationBe careful, it is a nonlinear operation



stochastic observer


Example: shuttle reentry I

We want to design an estimate for the state of a shuttle in reentryphase within the atmosphere at large altitude and large velocityWe define a reference frame y − x attached to the earth

x

y

[x4x3

]

[yrxr

]

[x2 − yrx1 − xr

]

x1 position along xx2 position along yx3 velocity along xx4 velocity along yx5 model parameter



stochastic observer


Example: shuttle reentry II

By definingr(t) =

√

x21(t) + x22(t)

v(t) =√

x23(t) + x24(t)

the c.t. model is

x1(t) = x3(t)x2(t) = x4(t)x3(t) = d(t)x3(t) + g(t)x1(t)x4(t) = d(t)x4(t) + g(t)x2(t)x5(t) = 0

where

d(t) = d(x1, x2, x5) = −β0ex5(t)e

r0−r(t)h0 v(t)

g(t) = − gm0

r3(t)Gianluca Antonelli Evry, 27-29 march 2013


stochastic observer


Example: shuttle reentry III

The measurement, obtained by a radar placed in xr, yr, gives

y1(t) =√

(x1(t)− xr)2 + (x2(t)− yr)2 + v1

y2(t) = atan2(

x2(t)−yrx1(t)−xr

)

+ v2

We know the data (proper unit measurmenets where absent):

β0 = 0.597h0 = 13.406 kmgm0 = 3.9860 · 105

xr = 6374 kmyr = 0 ⇒ r0 = xrT = 0.1 srv1 = 1rv2 = 17 · 10−3

x(0) =[6500 349 −1.8 −6.8 0.7

]T



stochastic observer


Example: shuttle reentry IV

The dynamic discretization gives

x1(k + 1) = x1(k) + Tx3(k)x2(k + 1) = x2(k) + Tx4(k)x3(k + 1) = (1 + Td(k))x3(k) + Tg(k)x1(k)x4(k + 1) = (1 + Td(k))x4(k) + Tg(k)x2(k)x5(k + 1) = x5(k)

on which it is possible to design a proper extended Kalman filter



stochastic observer


Example: shuttle reentry V

6300

6320

6340

6360

6380

6400

6420

6440

6460

6480

6500

-50 0 50 100 150 200 250 300 350

y [km]

x[km]

radar

Simulated reentry path



stochastic observer


Example: shuttle reentry VI

0

50

100

150

200

250

300

350

400

0 20 40 60 80 100 120 140 160 180 200

1.0

1.1

1.2

1.3

1.4

0 20 40 60 80 100 120 140 160 180 200

y 1[km]

y 2[rad

]

radar measurement

t [s]



stochastic observer


Example: shuttle reentry VII

6300

6320

6340

6360

6380

6400

6420

6440

6460

6480

6500

-50 0 50 100 150 200 250 300 350

y [km]

x[km]

radar

Simulated reentry path (black) and estimated (red)



stochastic observer


Example: shuttle reentry VIII

6380

6400

6420

6440

6460

6480

6500

6520

0 20 40 60 80 100 120 140 160 180 200

50

100

150

200

250

300

350

0 20 40 60 80 100 120 140 160 180 200

-7

-6

-5

-4

-3

-2

-1

0

1

0 20 40 60 80 100 120 140 160 180 200

0.595

0.600

0.605

0.610

0.615

0.620

0.625

0 20 40 60 80 100 120 140 160 180 200

y[km]

x[km]

x5[-]

x3,x

4[km/s]

x1 x2

x3, x4 x5 (only estimate)

t [s]



stochastic observer


Example: shuttle reentry IX

-5

-4

-3

-2

-1

0

1

2

3

0 20 40 60 80 100 120 140 160 180 200x1,x

2,x

3,x

4,x

5,[km],[km/s],[-]

x(k)

t [s]

Numerical example taken from: Austin, JW and Leondes, CT, Statisticallylinearized estimation of reentry trajectories, Aerospace and Electronic Systems,IEEE Transactions on, (1)54–61, 1981



stochastic observer


Kalman vs Luenberger

Luenberger is based on the deterministic model

In Luenberger we design the estimate convergence speed

Different interpretation of models/data

At steady state they are simbolically equal. When K(k) = K,Kalman becomes

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

P (k + 1) = P

K(k) = K

x(k) = x(k) +K [y(k)−Cx(k)]P (k) = P

thusx(k + 1) = Ax(k) +Bu(k) +K [y(k) − y(k)]



stochastic observer


Case study I

Position estimate via differential measurementsGiven x(k) ∈ R for which it is available a differential measurement

y(k) = x(k)− x(k − 1) + v(k)

one possible estimator is given by

x(k + 1) = x(k)︸︷︷︸

current estimate

+x(k + 1)− x(k) + v(k)︸︷︷︸

increment

with corresponding error dynamics:

e(k + 1) = x(k + 1)− x(k + 1)= x(k + 1)− x(k)− x(k + 1) + x(k)− v(k)= e(k) − v(k)



stochastic observer


Case study II

The error is thus a Brownian movement (random walking)

e(k + 1) = e(k) + v(k)

E[e(0)] = 0E[e2(0)] = 0E[e2(1)] = E[(e(0) + v(0))2]

= E[e2(0) + 2e(0)v(0) + v2(0)]= r2v

E[e2(2)] = E[(e(1) + v(1))2]= E[e2(1) + 2e(1)v(1) + v2(1)]= 2r2v...

E[e2(k)] = kr2v the variance grows!Gianluca Antonelli Evry, 27-29 march 2013


stochastic observer


Case study

Mapping

known vehicle position

relative measurement vehicle-landmark



stochastic observer


Case study

map built with the sole odometry



stochastic observer


Case study

Localization

landamark coordinates (map) known




stochastic observer


Case study

a localization example



stochastic observer


Case study

SLAM

strarting from an unkown intial position in an unknownenvironment, the robot should incrementally build a map whilelocating itself within the map



stochastic observer


Case study

SLAM

x is built with both the landmark and vehicle coordinates




stochastic observer


Case study I

x =[xv p1 . . . pN

]T xv ∈ R2 posizione del veicolo

pi ∈ R2 posizione del landmark

the dynamic model is thus given by:

xv(k + 1)p1(k + 1)

. . .pN (k + 1)

=

F (k) O · · · O

O I · · · O...

......

O O · · · I

xv(k)p1(k). . .

pN (k)

+

uv(k)0

. . .0

+

wv(k)w1(k). . .

wN (k)

the output is characterized by a time-varying matrix with generic

pi − xv



stochastic observer


Case study II

thus

y(k) = C(k)x(k) + v(k)

with the C(k) elements properly assumed as 1/0



stochastic observer


Case study

SLAM example



stochastic observer


Beyond Kalman

Kalman is optimal when the assumptions are met:

linearity

Gaussian noises

more complex situations require additional effort



stochastic observer


Exercise 1 I

Design a Kalman filter for the system

A =

0.9210 −0.1497 0.03520.0691 0.6841 0.0123−0.0842 0.2943 0.8525

B =

−2−1−1

C =[0 −3 −1

]


step input with unitary amplitude

consider using the file template kalman.sce downloadable fromthe website

run several simulations by varying measurement and processcovariances



stochastic observer


Exercise 2 I

Design a Kalman filter for the system

R

L

CVi

Vu

iLiC

with data

R = 680Ω

L = 1.5 · 10−3H (Rl = 0.4Ω)

C = 5 · 10−9F


square wave input with amplitude 3Volt e frequency 7KHz

final simulation time tf = 1milliseconds

sampling time T = 0.4microseconds

consider using the file template kalman.sce downloadable fromthe website



stochastic observer


Exercise 2 II

Validate again with a numerical simulatino and the real data stored inthe file RLC 02 small.txt

import the data in the workspace via the commandA=read(’RLC 02 small.txt’,-1,3), the second column stores theinput, the third the output

notice that the output needs to be properly transalted to take intoaccount an acquisition error



stochastic observer


Merci!


state estimate

Education

xt xt ek

t lim xt xt

gc xt f xt ek

observer structurec

lim xk xk

f ek gianluca antonellievry

f et ek

f xt ut gyt f xk uk