State estimate

introduction
deterministic observer
stochastic observer

A short introduction to
State Estimate

Gianluca Antonelli

Universit` degli Studi di Cassino e del Lazio Meridionale
a
antonelli@unicas.it
http://webuser.unicas.it/lai/robotica
http://www.eng.docente.unicas.it/gianluca antonelli

Gianluca Antonelli Evry, 27-29 march 2013

introduction
stochastic observer

Outline

Introduction
A naive (i.e., open loop) solution
The deterministic case (Luenberger)
The stochastic case (Kalman)


introduction
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
...

introduction
stochastic observer

Modeling
We will deal with ISO (Input State Output) dynamic system described
by diﬀerential or diﬀerence equations

continuos time discrete time

˙
x(t) = f (x(t), u(t), t) x(k + 1) = f (x(k), u(k), k)
y(t) = h(x(t), t) y(k) = h(x(k), k)

x ∈ Rn , u ∈ Rp , y ∈ Rm
assuming linearity and time invariance

c.t. d.t.

˙
x(t) = Ax(t) + Bu(t) x(k + 1) = Ax(k) + Bu(k)
y(t) = Cx(t) + Du(t) y(k) = Cx(k) + Du(k)

introduction
stochastic observer

Introduction
Measurements give an incomplete view of the state
The ﬁlter is required to estimate the state based on the model
knowledge

state measures ﬁlter


introduction
stochastic observer

Introduction

Is it possible to estimate the state of a dynamic system without direct
measurement of it?
u y
Σ

x?


introduction
stochastic observer

Open loop observer I

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

u y

Σ

x

ˆ
x
Σ


introduction
stochastic observer

Open loop observer II
Given an ISO system

c.t. d.t.

˙

x ∈ Rn , u ∈ Rp , y ∈ Rm
ˆ
deﬁning x(·) the x(·) estimate, we build

c.t. d.t.
˙
x(t) = Aˆ (t) + Bu(t)
ˆ x
ˆ
x(k + 1) = Aˆ (k) + Bu(k)
x
We deﬁne the error

introduction
stochastic observer

Open loop observer III

c.t. d.t.
ˆ
e(t) = x(t) − x(t) ˆ
e(k) = x(k) − x(k)
with dynamics:
c.t. d.t.

˙ ˙ ˙
e(t) = x(t) − x(t)
ˆ ˆ
e(k + 1) = x(k + 1) − x(k + 1)
= Ax(t) − Aˆ (t)
x = Ax(k) − Aˆ (k)
x
= Ae(t) = Ae(k)
and thus evolution:
c.t. d.t.
e(t) = eAt e(0) e(k) = Ak e(0)

introduction
stochastic observer

Open loop observer IV

which is unsuitable for several reasons
The estimate dynamics is related to the eigenvalues of A and it
can not be modiﬁed
It is necessary to assume Σ asymptotically stable
Knowledge of the output (a linear combination of the state) is not
exploited
ˆ
The system is known with an identiﬁcation error, only Σ is
available


introduction problem formulation
deterministic observer solution
stochastic observer exercises

Closed loop observer (Luenberger)

It is required to estimate the state by exploiting both the input and
the output
u y

Σ

x

ˆ
x
obs



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

Given and ISO system

c.t. d.t.

˙

x ∈ Rn , u ∈ Rp , y ∈ Rm
and the observer structure
c.t. d.t.

˙
ˆ ˆ
x(t) = F x(t) + Γ u(t) + Gy(t) ˆ ˆ
x(k+1) = F x(k)+Γ u(k)+Gy(k)



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

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

c.t. d.t.
 
 t→∞ ˆ
 lim x(t) − x(t) = 0 ˆ
 lim x(k) − x(k) = 0
 k→∞

 ∀u(t), ∀x(0), ∀ˆ (0) 
 ∀u(k), ∀x(0), ∀ˆ (0)
x x

i.e., that the estimate converge to the true value for all the initial
conditions of both the system and the observer and for all the possible
inputs



Solution c.t. and d.t. I

Deﬁning the error

c.t. d.t.
ˆ
e(t) = x(t) − x(t) ˆ
e(k) = x(k) − x(k)

with dynamics:
c.t. d.t.

˙ ˙ ˙
e(t) = x(t) − x(t)
ˆ ˆ
e(k + 1)=x(k + 1) − x(k + 1)
= Ax(t) + Bu(t)+ = Ax(k) + Bu(k)+
ˆ
−F x(t) − Γ u(t) − Gy(t) ˆ
−F x(k) − Γ u(k) − Gy(k)

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



Solution c.t. and d.t. II
assuming D = O and substituting y = Cx, it holds

c.t. d.t.

˙ ˆ
e(t) = (A − GC) x(t) − F x(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. d.t.
˙
e(t) = F e(t) e(k + 1) = F e(k)



Solution c.t. and d.t. III
and thus the evolution:
c.t. d.t.
e(t) = eF t e(0) e(k) = F k e(0)
It is thus necessary that the eigenvalues of the dynamic matrix F all
stay in the stability region so that the error converges to zero
We 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 coeﬃcients of the desired characteristic polynomium:
n
(λ − λd,i ) = λn + d1 λn−1 + . . . + dn
i=1


Solution c.t. and d.t. IV

The characteristic polynomium

|λI − (A − GC)|

is thus ﬁxed
The 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 or
column vectors c and g
We compute the characteristic polynomium of A

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



Solution c.t. and d.t. V
We compute the matrix:
 
an−1 an−2 . . . a1 1
an−2 an−3 ... 1 0
n−1 T  
T = c T AT c T . . . AT c  ...
 ... . . . . . . . . .

 ... ... . . . . . . . . .
1 0 ... 0 0

that is full rank if the ﬁrst matrix is, i.e., O T (the transpose of the
observability matrix)
By deﬁning
T
d = dn . . . d1
T
a = an . . . a1



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



Selection of the F eigenvalues
The observer needs to converge as fast as possible to the true value
Theoretically the eigenvalues may be placed everywhere in the complex
plane
In practice, a bound is given by the measurements noise

c.t. d.t.
I I
λa

λa
λd R λd R
1



Model knowledge

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

F ˆ ˜
= A − GC = A − A − GC
Γ ˆ
= B =B−B ˜

the error evolution is thus characterized by:

c.t. d.t.

˙ ˜x ˜
e(t) = F e(t) + Aˆ (t) + Bu(t) ˜x ˜
e(k +1) = F e(k)+ Aˆ (k)+ Bu(k)
that is not anymore homogeneous
˜
(A may exhibits unstable eigenvalues, it is an issue?)



Reduced observer I
If the system is not observable it is possible to implement a reduced
observer that gives and estimate of the sole observable component
By resorting to a proper matrix of equivalence the system is
transformed into the canonical form
c.t. d.t.
 
Ao O Ao O
˙
z(t) = z(t) + P −1 Bu(t) z(k + 1) = z(k) + P −1 Bu(k)
 
A1 Ano A1 Ano
y(t) = C o O z(t) y(k) = C o O z(k)
 



Reduced observer II

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

c.t. d.t.

˙
z o (t) = Ao z o (t) + P −1 B(1 : o, :)u(t) z o (k + 1) = Ao z o (k) + P −1 B(1 : o, :)u(k)
y(t) = C o z o (t) y(k) = C o z o (k)

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



A feedback interpretation
The observer may be rewritten as

c.t. d.t.

˙ x ˆ
x(t) = Aˆ (t) + Bu(t) + G (y(t) − C x(t))
ˆ ˆ ˆ
x(k+1) = Aˆ (k)+Bu(k)+G (y(k) − C x(k))
x

thus
c.t. d.t.

˙ x ˆ
x(t) = Aˆ (t) + Bu(t) + G (y(t) − y (t))
ˆ ˆ ˆ
x(k+1) = Aˆ (k) + Bu(k) + G (y(k) − y (k))
x
emulation feedback emulation feedback

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


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 proper
commands of numerical software under the description pole placement
(ex: ppol in Scilabor place in Matlab)



Extended Luenberger ﬁlter I

Assuming a nonlinear dynamic system

c.t. d.t.

˙
x(t) = f (x(t), u(t), t) x(k + 1) = f (x(k), u(k), k)
y(t) = h(x(t), t) y(k) = h(x(k), k)

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

c.t. d.t.

˙ x ˆ
x(t) = f (ˆ (t), u(t), t) + G (y(t) − y (t))
ˆ ˆ ˆ
x(k+1) = f (ˆ (k), u(k), k) + G (y(k) − y (k))
x
emulation feedback emulation feedback



Extended Luenberger ﬁlter II

where
ˆ
y (·) = h(ˆ (·), ·)
x
and the gain, both for the c.t. and the d.t., is computed by resorting to
the same formulas of the linear design where matrices A and C are
computed by linearization around the current estimate:
∂
C(·) = ∂ x h(x) x=x(·)
ˆ

∂
A(·) = ∂ x f (x) x=x(·)
ˆ



Exercise 1 I

Design a Luenberger observer for the system
   


 0.9210 −0.1497 0.0352 −2

A =  0.0691 0.6841 0.0123 B = −1

 −0.0842 0.2943 0.8525 −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 the
observer
plot in the oscilloscope both the states and the estimate error



Exercise 2 I

Design a Luenberger observer for the RLC circuit
L with data
iL
iC Vu R = 680Ω
Vi R C L = 1.5 · 10−3 H (Rl = 0.4Ω)
−9
C = 5 · 10 F
square wave input with amplitude 3 Volt e frequency 7 KHz
ﬁnal simulation time tf = 1 milliseconds
sampling time T = 0.4 microseconds
build two superblocks, one for the real model and one for the
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 in
the ﬁle RLC 02 small.txt
consider the sole observer superblock developed
import the data in the workspace via the command
A=read(’RLC 02 small.txt’,-1,3), the second column stores the
input, the third the output
read input and output of the real model from the workspace woth
the block From Workspace
notice that the output needs to be properly transalted to take into
account an acquisition error


stochastic observer examples and exercises

Deﬁnitions on stochastic variables I

Given a stochastic variabile x ∈ Rn with probability density
function fx (x) its expected value (average) is

E[x] = µx = xfx (x)dx ∈ Rn
Rn

The covariance matrix is deﬁned as

P x = E (x − µx )(x − µx )T ∈ Rn×n



Deﬁnitions on stochastic variables II
A multivariate Gaussian distribution is the function
1
e− 2 (x−µx ) P x (x−µx )
1 T
fx (x) =
(2π)n P
x

In the bidimensional case it exhibits a graphical interpretation

average µx
µx isopotential curves are ellipses
the principal axes of the ellipses
are parallel to the eigenvectors
v i of P x
the square root of a principal
v2 v1 axis is equal to the
corresponding eigevalue



Deﬁnitions on stochastic variables III

x1
x2 v2
v1
µx



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)

where
x ∈ Rn stato
y ∈ Rm uscita
u ∈ Rp ingresso
w ∈ Rn rumore di processo
v ∈ Rm rumore di misura
ﬁnd an optimal estimation for x(k)



Linear model

Let us consider ﬁrst 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, l
E[xh (i)vl (j)] = 0 per ogni i, j, h, l



Variable deﬁnition

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

✻ˆ ✻ˆ
x(k) x(k+1) x(k+1)
temporal update
✲
P (k) P (k+1) P (k+1)

k k+1


Kalman ﬁlter equations


temporal update  x(k + 1)
 = Aˆ (k) + Bu(k)
x
(prediction) 
 P (k + 1)

 = AP (k)AT + Rw
−1
measurement update K(k) = P (k)C T Rv + CP (k)C T

(correction)  ˆ
 x(k)
 = x(k) + K(k) [y(k) − Cx(k)]

 P (k) = P (k) − K(k)CP (k)

Update equations not necessarily synchronous
Possibility to fuse/merge diﬀerent measurement sources
Suitable to be implemented for the non stationary case



Sketch

model measurement

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

prediction correction



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 ﬁrst absence of noise, it allows to write the following
minimization problem

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


Gain geometric interpretation II

where
∆x P = ∆xT P (k)−1 ∆x
Notice
the equation y = Cx represents an aﬃne subspace of dimension m
in the state space
the Mahalanobis norm ∆x P wheigths more the better estimated
state components
the problem is underconstrained ⇒ inﬁnite solutions ∆x exists
the state is a stochastic variable with probability density function
of kind Gaussian multivariate



Gain geometric interpretation III

The minimum norm solution is
−1
∆x = C † (y(k) − Cx(k)) = P C T CP C T (y(k) − Cx(k))

By adding measurement noise with covariance Rv one obtains the gain
formula
−1
∆x = P C T CP C T + Rv (y(k) − Cx(k))



Gain geometric interpretation
In the numeric case:

x2

x(k)
n = 2
m = 1
y(k) = Cx(k) C = 1 1

x1



without noise and symmetric covariance

x2
n = 2
m = 1
x(k)
C = 1 1
∆x 1 0
P (k) =
0 1
y(k) = Cx(k)
Rv = 0
1
K(k) =
1
x1



without noise and asymmetric covariance

x2
n = 2
m = 1
x(k)
C = 1 1
∆x 1 0
P (k) =
0 2
y(k) = Cx(k)
Rv = 0
1
K(k) = 13 2
x1



with noise the subspace is uncertain:

x2
n = 2
m = 1
x(k)
C = 1 1
∆x 1 0
P (k) =
0 2
y(k) = Cx(k)
Rv = 1
1
K(k) = 14 2
x1



Covariance dynamics

The ﬁlter may be interpreted even as propagation of the
prediction-correction estimate average value and covariance

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

with

x(k) ˆ
x(k)
Rw = rw = 1

Rv = rv = 0.5

ˆ
x(k)
x(k + 1)


Gain matrix I
An alternative formulation

K(k) = P (k)C T Rv
−1

where it is possible to notice
Proportional to the error covariance
Inversely proportional to the measurement error covariance
The process noise covariance inﬂuences P (k)
when Rw = O ⇒ P (k) → O and K(k) → O
The relative wheight between measurement and process noise
covariances represents the relative trust between model and
measurement
Good model ⇒ small gain
Good measurement ⇒ large gain



Gain matrix II

Under assumptions of
linearity
stazionarity
observability
reachability
K(k) → K ∞
with K ∞ solution of the Riccati equation
−1
K ∞ = P ∞ C T Rv + CP ∞ C T



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 ﬁlter



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 ﬁlter
The hortogonality between estimate and error holds

E x(k)˜ (k)T = O
ˆ x



Stability

The state estimate evolves following the dynamics

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

An anlytical proof requires hard constraints
When the system is observable E[˜ (k)] is bounded ∀k
x
Stable for most of practical applications



Example: estimate of a constant I
We have at disposal the noisy measurement of a constant with trivial
dynamic

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



Example: estimate of a constant II
The update equations of the error covariance and of the gain are:

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

(the scalar gain K is now scalar but we keep it in boldface to diﬀerentiate it from
the time)



Example: estimate of a constant III
At the initial instant we have:
p0
K(0) =
rv + p0
p 0 rv
p(0) =
rv + p0
p(1) = p(0)

for k = 1 we got:

p(1) p(0) p0 rv p0
K(1) = = = 2 =
rv + p(1) rv + p(0) rv + p0 rv + p0 rv rv + 2p0
p0 rv
p(1) =
rv + 2p0
p0 rv
p(2) =
rv + 2p0


Example: estimate of a constant IV
by iteration it is possible to compute symbolically the gain as
p0 p0 /rv
K(k) = =
rv + (k + 1)p0 1 + (k + 1)p0 /rv
the estimate is then given by:
p0 /rv
x(k) = x(k) +
ˆ [y(k) − x(k)]
1 + (k + 1)p0 /rv
where we notice
for increasing k, the new measurements are not used anymore to
update the estimate
the gain is strongly aﬀected by the ration between p0 and rv
to avoid that the gain tends to zero it is necessary to introduce a
process noise


Extended Kalman ﬁlter

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

ˆ
C(k) = ∂
∂ x h(x) x=x(k)
ˆ
A(k) = ∂
∂ x f (x) x=x(k)
ˆ

−1
K(k) ˆ ˆ ˆ
= P (k)C(k)T Rv + C(k)P (k)C(k)T
ˆ
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 (ˆ (k), k) + Bu(k)
x
P (k + 1) = ˆ ˆ
A(k)P (k)A(k)T + Rw



Example: estimate of the model’s parameters I
Possible use: estimate the value of the constant value of the parameter
of a physical model
Let us assume θ as the vector of the model unknown parameters
The 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 of
the system with dynamics:

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

Let us deﬁne the extended state vector:
x
xE =
θ


Example: estimate of the model’s parameters II

the moidel becomes:

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

where
w
wE =
wθ
The noise wθ represents the uncertainty of the parameter’s value
The ﬁlter admits thus its variation
Be careful, it is a nonlinear operation



Example: shuttle reentry I
We want to design an estimate for the state of a shuttle in reentry
phase within the atmosphere at large altitude and large velocity
We deﬁne a reference frame y − x attached to the earth

x
x4
x3
x1 position along x
x2 − y r x2 position along y
x1 − xr x3 velocity along x
x4 velocity along y
x5 model parameter
yr
xr y


Example: shuttle reentry II
By deﬁning
r(t) = x2 (t) + x2 (t)
1
2

v(t) = x2 (t) + x4 (t)
3
2

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
r0 −r(t)
d(t) = d(x1 , x2 , x5 ) = −β0 ex5 (t) e h0
v(t)

g(t) = − gm0
r 3 (t)


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
x2 (t)−yr
y2 (t) = atan2 x1 (t)−xr + v2
We know the data (proper unit measurmenets where absent):
β0 = 0.597
h0 = 13.406 km
gm0 = 3.9860 · 105
xr = 6374 km
yr = 0 ⇒ r0 = xr
T = 0.1 s
rv1 = 1
rv2 = 17 · 10−3
T
x(0) = 6500 349 −1.8 −6.8 0.7


Example: shuttle reentry IV
The dynamic discretization gives

x1 (k + 1) = x1 (k) + T x3 (k)
x2 (k + 1) = x2 (k) + T x4 (k)
x3 (k + 1) = (1 + T d(k))x3 (k) + T g(k)x1 (k)
x4 (k + 1) = (1 + T d(k))x4 (k) + T g(k)x2 (k)
x5 (k + 1) = x5 (k)

on which it is possible to design a proper extended Kalman ﬁlter



Example: shuttle reentry V

6500
Simulated reentry path
6480

6460

6440
x [km]

6420

6400

6380 radar
6360

6340

6320

6300
-50 0 50 100 150 200 250 300 350

y [km]



Example: shuttle reentry VI
400
radar measurement
350
y1 [km]
300

250

200

150

100

50

0
0 20 40 60 80 100 120 140 160 180 200

1.4
y2 [rad]

1.3

1.2

1.1

1.0

0 20 40 60 80 100 120 140 160 180 200

t [s]



Example: shuttle reentry VII

Simulated reentry path (black) and estimated (red)
6500

6480

6460

6440
x [km]

6420

6400

6380 radar
6360

6340

6320

6300
-50 0 50 100 150 200 250 300 350

y [km]



Example: shuttle reentry VIII
6520
x1 350
x2
6500
300

x [km]
y [km]

6480
250
6460
200
6440
150
6420

6400 100

6380 50
0 20 40 60 80 100 120 140 160 180 200 0 20 40 60 80 100 120 140 160 180 200

1
x3 , x4 0.625
x5 (only estimate)
x3 ,x4 [km/s]

0
0.620
-1
0.615
-2 x5 [-]
-3 0.610

-4
0.605
-5
0.600
-6

-7 0.595
0 20 40 60 80 100 120 140 160 180 200 0 20 40 60 80 100 120 140 160 180 200

t [s]



Example: shuttle reentry IX

x1 ,x2 ,x3 ,x4 ,x5 , [km],[km/s],[-]
3
˜
x(k)
2

1

0

-1

-2

-3

-4

-5
0 20 40 60 80 100 120 140 160 180 200

t [s]

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


Kalman vs Luenberger

Luenberger is based on the deterministic model
In Luenberger we design the estimate convergence speed
Diﬀerent interpretation of models/data
At steady state they are simbolically equal. When K(k) = K,
Kalman becomes

 x(k + 1)
 = Aˆ (k) + Bu(k)
x

 P (k + 1)
 = P
K(k) = K

 ˆ
 x(k)
 = x(k) + K [y(k) − Cx(k)]

P (k) = P

thus
ˆ ˆ
x(k + 1) = Aˆ (k) + Bu(k) + K [y(k) − y (k)]
x


Case study I
Position estimate via diﬀerential measurements
Given x(k) ∈ R for which it is available a diﬀerential measurement

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

one possible estimator is given by

x(k + 1) =
ˆ x(k)
ˆ + x(k + 1) − x(k) + v(k)
current estimate 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)



Case study II
The error is thus a Brownian movement (random walking)
e(k + 1) = e(k) + v(k)

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

= E[e2 (1) + 2e(1)v(1) + v 2 (1)]
2
= 2rv
.
.
.
E[e2 (k)] = krv
2
the variance grows!


Case study
Mapping
known vehicle position
relative measurement vehicle-landmark



Case study
map built with the sole odometry



Case study
Localization
landamark coordinates (map) known



Case study

a localization example



Case study

SLAM
strarting from an unkown intial position in an unknown
environment, the robot should incrementally build a map while
locating itself within the map



Case study
SLAM
x is built with both the landmark and vehicle coordinates



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:
  F (k) O · · · 
O  xv (k)  uv (k)  wv (k) 
xv (k + 1)
 p1 (k + 1)   O I ··· O   p1 (k)   0   w1 (k) 
 = . .

.  ...  +  ...  +  ... 
 ...   .
 . .
. . 
.     
pN (k + 1) O O ··· I pN (k) 0 wN (k)

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

p i − xv



Case study II

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

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



Case study
SLAM example



Beyond Kalman

Kalman is optimal when the assumptions are met:
linearity
Gaussian noises

more complex situations require additional eﬀort



Exercise 1 I

Design a Kalman ﬁlter for the system
   


 0.9210 −0.1497 0.0352 −2

A =  0.0691 0.6841 0.0123 B = −1

 −0.0842 0.2943 0.8525 −1

C = 0 −3 −1

step input with unitary amplitude
consider using the ﬁle template kalman.sce downloadable from
the website
run several simulations by varying measurement and process
covariances



Exercise 2 I

Design a Kalman filter for the system
L with data
iL
iC Vu R = 680Ω
Vi R C L = 1.5 · 10−3 H (Rl = 0.4Ω)
−9
C = 5 · 10 F
square wave input with amplitude 3 Volt e frequency 7 KHz
final simulation time tf = 1 milliseconds
sampling time T = 0.4 microseconds
consider using the file template kalman.sce downloadable from
the website



Exercise 2 II

Validate again with a numerical simulatino and the real data stored in
the ﬁle RLC 02 small.txt
import the data in the workspace via the command
A=read(’RLC 02 small.txt’,-1,3), the second column stores the
input, the third the output
notice that the output needs to be properly transalted to take into
account an acquisition error



Merci!


State estimate

More Related Content

What's hot

Viewers also liked

Similar to State estimate

More from Gianluca Antonelli

Recently uploaded

State estimate