Upload
gianluca-antonelli
View
773
Download
1
Tags:
Embed Size (px)
DESCRIPTION
A short introduction to state estimate, deterministic and stochastic cases
Citation preview
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
introductiondeterministic observer
stochastic observer
Outline
Introduction
A naive (i.e., open loop) solution
The deterministic case (Luenberger)
The stochastic case (Kalman)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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
. . .
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
Introduction
Is it possible to estimate the state of a dynamic system without directmeasurement of it?
u y
x ?
Σ
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
Open loop observer I
We build a copy of the system, feeded in parallel with the same input
uy
x
x
Σ
Σ
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
Open loop observer II
Given an ISO system
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)
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
Problem formulation c.t. and d.t. I
Given and ISO system
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)
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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?)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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(·)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Validate with a numerical simulation
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Definitions on stochastic variables III
x1
x2
µx
v1
v2
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Sketch
x(k) x(k + 1) x(k + 1)
prediction correction
model measurement
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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))
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Gain geometric interpretation
In the numeric case:
x1
x2
x(k)
y(k) = Cx(k)
n = 2m = 1C =
[1 1
]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Gain geometric interpretation
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
]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Gain geometric interpretation
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
]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Gain geometric interpretation
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
]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
θ
]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)]
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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)
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
Mapping
known vehicle position
relative measurement vehicle-landmark
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
map built with the sole odometry
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
Localization
landamark coordinates (map) known
relative measurement vehicle-landmark
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
a localization example
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
SLAM
strarting from an unkown intial position in an unknownenvironment, the robot should incrementally build a map whilelocating itself within the map
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
SLAM
x is built with both the landmark and vehicle coordinates
relative measurement vehicle-landmark
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study II
thus
y(k) = C(k)x(k) + v(k)
with the C(k) elements properly assumed as 1/0
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Case study
SLAM example
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Beyond Kalman
Kalman is optimal when the assumptions are met:
linearity
Gaussian noises
more complex situations require additional effort
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
]
Validate with a numerical simulation
step input with unitary amplitude
consider using the file template kalman.sce downloadable fromthe website
run several simulations by varying measurement and processcovariances
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Validate with a numerical simulation
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
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
Gianluca Antonelli Evry, 27-29 march 2013
introductiondeterministic observer
stochastic observer
problem formulationsolutionexamples and exercises
Merci!
Gianluca Antonelli Evry, 27-29 march 2013