# Lecture 8 The Kalman ï¬lter - Stanford University

• View
1

0

Embed Size (px)

### Text of Lecture 8 The Kalman ï¬lter - Stanford University

Linear system driven by stochastic process
we consider linear dynamical system xt+1 = Axt + But, with x0 and u0, u1, . . . random variables
we’ll use notation
and similarly for ut, Σu(t)
taking expectation of xt+1 = Axt + But we have
xt+1 = Axt + But
i.e., the means propagate by the same linear dynamical system
The Kalman filter 8–2
now let’s consider the covariance
xt+1 − xt+1 = A(xt − xt) + B(ut − ut)
and so
Σx(t + 1) = E (A(xt − xt) + B(ut − ut)) (A(xt − xt) + B(ut − ut)) T
= AΣx(t)AT + BΣu(t)BT + AΣxu(t)BT + BΣux(t)AT
T
The Kalman filter 8–3
consider special case Σxu(t) = 0, i.e., x and u are uncorrelated, so we have Lyapunov iteration
Σx(t + 1) = AΣx(t)AT + BΣu(t)BT ,
which is stable if and only if A is stable
if A is stable and Σu(t) is constant, Σx(t) converges to Σx, called the steady-state covariance, which satisfies Lyapunov equation
Σx = AΣxAT + BΣuBT
thus, we can calculate the steady-state covariance of x exactly, by solving a Lyapunov equation
(useful for starting simulations in statistical steady-state)
The Kalman filter 8–4
Example
A =
where wt are IID N (0, I)
eigenvalues of A are 0.6 ± 0.75j, with magnitude 0.96, so A is stable
we solve Lyapunov equation to find steady-state covariance
Σx =
]
covariance of xt converges to Σx no matter its initial value
The Kalman filter 8–5
two initial state distributions: Σx(0) = 0, Σx(0) = 102I
plot shows Σ11(t) for the two cases
0 10 20 30 40 50 60 70 80 90 100 0
20
40
60
80
100
120
t
(xt)1 for one realization from each case:
0 10 20 30 40 50 60 70 80 90 100 −15
−10
−5
0
5
10
15
0 10 20 30 40 50 60 70 80 90 100 −15
−10
−5
0
5
10
15
t
Linear Gauss-Markov model
xt+1 = Axt + wt, yt = Cxt + vt
• xt ∈ Rn is the state; yt ∈ Rp is the observed output
• wt ∈ Rn is called process noise or state noise
• vt ∈ Rp is called measurement noise
w x y v
Statistical assumptions
• x0, w0, w1, . . ., v0, v1, . . . are jointly Gaussian and independent
• wt are IID with Ewt = 0, Ewtw T t = W
• vt are IID with E vt = 0, E vtv T t = V
• Ex0 = x0, E(x0 − x0)(x0 − x0) T = Σ0
(it’s not hard to extend to case where wt, vt are not zero mean)
we’ll denote Xt = (x0, . . . , xt), etc.
since Xt and Yt are linear functions of x0, Wt, and Vt, we conclude they are all jointly Gaussian (i.e., the process x, w, v, y is Gaussian)
The Kalman filter 8–9
Statistical properties
• wt is independent of x0, . . . , xt and y0, . . . , yt
• Markov property: the process x is Markov, i.e.,
xt|x0, . . . , xt−1 = xt|xt−1
roughly speaking: if you know xt−1, then knowledge of xt−2, . . . , x0
The Kalman filter 8–10
Mean and covariance of Gauss-Markov process
mean satisfies xt+1 = Axt, Ex0 = x0, so xt = Atx0
covariance satisfies
Σx(t + 1) = AΣx(t)AT + W
if A is stable, Σx(t) converges to steady-state covariance Σx, which satisfies Lyapunov equation
Σx = AΣxAT + W
Conditioning on observed output
we use the notation
Σt|s = E(xt − xt|s)(xt − xt|s) T
• the random variable xt|y0, . . . , ys is Gaussian, with mean xt|s and covariance Σt|s
• xt|s is the minimum mean-square error estimate of xt, based on y0, . . . , ys
• Σt|s is the covariance of the error of the estimate xt|s
The Kalman filter 8–12
State estimation
we focus on two state estimation problems:
• finding xt|t, i.e., estimating the current state, based on the current and past observed outputs
• finding xt+1|t, i.e., predicting the next state, based on the current and past observed outputs
since xt, Yt are jointly Gaussian, we can use the standard formula to find xt|t (and similarly for xt+1|t)
xt|t = xt + ΣxtYt Σ−1
Yt (Yt − Yt)
the inverse in the formula, Σ−1 Yt
, is size pt × pt, which grows with t
the Kalman filter is a clever method for computing xt|t and xt+1|t
recursively
Measurement update
let’s find xt|t and Σt|t in terms of xt|t−1 and Σt|t−1
start with yt = Cxt + vt, and condition on Yt−1:
yt|Yt−1 = Cxt|Yt−1 + vt|Yt−1 = Cxt|Yt−1 + vt
since vt and Yt−1 are independent
[
CΣt|t−1 CΣt|t−1C T + V
]
now use standard formula to get mean and covariance of
(xt|Yt−1)|(yt|Yt−1),
which is exactly the same as xt|Yt:
xt|t = xt|t−1 + Σt|t−1C T
(
Σt|t = Σt|t−1 − Σt|t−1C T
(
)−1 CΣt|t−1
this gives us xt|t and Σt|t in terms of xt|t−1 and Σt|t−1
this is called the measurement update since it gives our updated estimate of xt based on the measurement yt becoming available
The Kalman filter 8–15
Time update
now let’s increment time, using xt+1 = Axt + wt
condition on Yt to get
xt+1|Yt = Axt|Yt + wt|Yt
= Axt|Yt + wt
therefore we have xt+1|t = Axt|t and
Σt+1|t = E(xt+1|t − xt+1)(xt+1|t − xt+1) T
= E(Axt|t − Axt − wt)(Axt|t − Axt − wt) T
= AΣt|tA T + W
Kalman filter
measurement and time updates together give a recursive solution
start with prior mean and covariance, x0|−1 = x0, Σ0|−1 = Σ0
apply the measurement update
(
Σt|t = Σt|t−1 − Σt|t−1C T
(
)−1 CΣt|t−1
to get x0|0 and Σ0|0; then apply time update
xt+1|t = Axt|t, Σt+1|t = AΣt|tA T + W
to get x1|0 and Σ1|0
now, repeat measurement and time updates . . .
The Kalman filter 8–17
Riccati recursion
we can express measurement and time updates for Σ as
Σt+1|t = AΣt|t−1A T + W − AΣt|t−1C
T (CΣt|t−1C T + V )−1CΣt|t−1A
T
which is a Riccati recursion, with initial condition Σ0|−1 = Σ0
• Σt|t−1 can be computed before any observations are made
• thus, we can calculate the estimation error covariance before we get any observed data
The Kalman filter 8–18
Comparison with LQR
in LQR,
• Riccati recursion for Pt (which determines the minimum cost to go from a point at time t) runs backward in time
• we can compute cost-to-go before knowing xt
in Kalman filter,
• Riccati recursion for Σt|t−1 (which is the state prediction error covariance at time t) runs forward in time
• we can compute Σt|t−1 before we actually get any observations
The Kalman filter 8–19
Observer form
we can express KF as
xt+1|t = Axt|t−1 + AΣt|t−1C T (CΣt|t−1C
T + V )−1(yt − Cxt|t−1)
= Axt|t−1 + Lt(yt − yt|t−1)
where Lt = AΣt|t−1C T (CΣt|t−1C
T + V )−1 is the observer gain
• yt|t−1 is our output prediction, i.e., our estimate of yt based on y0, . . . , yt−1
• et = yt − yt|t−1 is our output prediction error
• Axt|t−1 is our prediction of xt+1 based on y0, . . . , yt−1
• our estimate of xt+1 is the prediction based on y0, . . . , yt−1, plus a linear function of the output prediction error
The Kalman filter 8–20
Kalman filter block diagram
The Kalman filter 8–21
as in LQR, Riccati recursion for Σt|t−1 converges to steady-state value Σ, provided (C, A) is observable and (A, W ) is controllable
Σ gives steady-state error covariance for estimating xt+1 given y0, . . . , yt
note that state prediction error covariance converges, even if system is unstable
Σ satisfies ARE
(which can be solved directly)
The Kalman filter 8–22
steady-state filter is a time-invariant observer:
xt+1|t = Axt|t−1 + L(yt − yt|t−1), yt|t−1 = Cxt|t−1
where L = AΣCT (CΣCT + V )−1
define state estimation error xt|t−1 = xt − xt|t−1, so
yt − yt|t−1 = Cxt + vt − Cxt|t−1 = Cxt|t−1 + vt
and
= Axt + wt − Axt|t−1 − L(Cxt|t−1 + vt)
= (A − LC)xt|t−1 + wt − Lvt
The Kalman filter 8–23
thus, the estimation error propagates according to a linear system, with closed-loop dynamics A − LC, driven by the process wt − LCvt, which is IID zero mean and covariance W + LV LT
provided A, W is controllable and C, A is observable, A − LC is stable
The Kalman filter 8–24
Example
with xt ∈ R6, yt ∈ R
we’ll take Ex0 = 0, Ex0x T 0 = Σ0 = 52I; W = (1.5)2I, V = 1
eigenvalues of A:
(which have magnitude one)
The Kalman filter 8–25
first let’s find variance of yt versus t, using Lyapunov recursion
E y2 t = CΣx(t)CT + V, Σx(t + 1) = AΣx(t)AT + W, Σx(0) = Σ0
0 20 40 60 80 100 120 140 160 180 200 0
2000
4000
6000
8000
10000
12000
14000
16000
18000
t
now, let’s plot the prediction error variance versus t,
E e2 t = E(yt|t−1 − yt)
2 = CΣt|t−1C T + V,
where Σt|t−1 satisfies Riccati recursion, initialized by Σ−1|−2 = Σ0,
Σt+1|t = AΣt|t−1A T + W − AΣt|t−1C
T (CΣt|t−1C T + V )−1CΣt|t−1A
T
0 20 40 60 80 100 120 140 160 180 200 17
17.5
18
18.5
19
19.5
20
20.5
t
The Kalman filter 8–27
now let’s try the Kalman filter on a realization yt
top plot shows yt; bottom plot shows et (on different vertical scale)
0 20 40 60 80 100 120 140 160 180 200 −300
−200
−100
0
100
200
300
0 20 40 60 80 100 120 140 160 180 200 −30
−20
−10
0
10
20
30
t
t ##### Coarse-scale constrained ensemble Kalman lter for ...isc.tamu.edu/resources/preprints/2008/2008-06.pdfCoarse-scale constrained ensemble Kalman lter for subsurface characterization
Documents ##### Robustness Evaluation of Extended and Unscented Kalman ... ˝lter (EKF) and unscented Kalman ˝lter (UKF) for state of charge (SOC) estimation of a lithium-ion battery against unknown
Documents ##### State Space Models and the Kalman State Space Models and the Kalman Filter What we did last time: I The scalar lter I Combining period t prior and signal is analogous to a simple minimum
Documents ##### Estimating Exponential A¢ ne Models With Correlated Measurement Errors:Applications ... · PDF file 2011. 12. 20. · of Cox-Ingersoll-Ross (CIR) type, the extended Kalman –lter
Documents ##### Sequential Bayesian Updating - Oxford steffen/teaching/bs2HT9/kalman.pdf · PDF fileFixed state Evolving state Kalman lter Particle lters Sequential Bayesian Updating Ste en Lauritzen,
Documents ##### ANALYSIS OF THE ENSEMBLE KALMAN FILTER FOR INVERSE · PDF file 2017-08-23 · Key words. Bayesian inverse problems, ensemble Kalman lter, optimization AMS subject classi cations. 65N21,
Documents ##### State Space Models and the Kalman · PDF file 2016-02-24 · What determines the Kalman gain k t? Kalman lter optimally combine information in prior ˆx t 1jt 1 and signal z tto form
Documents ##### Distributed Agent-based Dynamic State Estimation over a ...ceur-ws.org/Vol-1156/  · PDF file cept is unchanged, the distributed Kalman lter has evolved through numerous algorithms
Documents ##### Performance study of Kalman Filter track reconstruction ... · PDF file Kalman lter, a Kalman lter with reference track and one Deterministic Annealing Filter (DAF). At present, the
Documents ##### Empirical Bayes improvement of Kalman lter type of pluto.mscc.huji.ac.il/~yaacov/state-  · PDF fileEmpirical Bayes improvement of Kalman lter type of estimators ... assumptions hold
Documents ##### Overview of localization techniques for ensemble based Kalman · PDF file 2013. 7. 15. · Overview of localization techniques for ensemble based Kalman lter algorithms T.Janji c Alfred
Documents ##### arXiv:1805.06198v2 [ ] 5 Feb 2019 · PDF file Navigation-compatible hybrid quantum accelerometer using a Kalman lter Pierrick Cheiney,1,2, Lauriane Fouch e,2, ySimon Templier,1,2 Fabien
Documents ##### Research Article Kalman Filter Sensor Fusion for Mecanum · PDF file 2019. 7. 31. · Mecanum AGV. In Section , we describe the Kalman lter sensor fusion algorithms and system modeling
Documents ##### 1 Particle Filters For Positioning, Navigation And · PDF fileParticle Filters for Positioning, Navigation and Tracking ... enabling a Kalman lter to estimate ... or even replacement
Documents ##### Chapter 5 · PDF file Chapter 5 The Linear Kalman Filter In this lecture w ederiv e and study the Kalman lter and its prop erties for the case of time{discrete dynamics and time{discrete
Documents ##### Kalman filter demystified: from intuition to E. Benhamou/Kalman lter demysti ed 3 Fig 1: A simple example from nancial markets to build an intuition of Kalman lter From this simple
Documents ##### Multiple Target Tracking Using Frame Multiple Target Tracking Using Frame Triplets 3 2 Related Work Recursive ltering approaches for single target tracking, such as the Kalman lter
Documents ##### Ein Kontur sch atzendes Kalman lter mithilfe der ... fileEin Kontur sch atzendes Kalman lter mithilfe der Evidenztheorie Sebastian Ohl und Markus Maurer y Montag 14 Februar, 2011 Zusammenfassung:
Documents ##### Attitude Navigation using a Sigma-Point Kalman Filter in ... 1141205/FULLTEXT01.pdf · PDF file2 MATLAB Implementation of an Unscented Kalman Filter 3 ... EKF extended Kalman lter
Documents ##### Expectation Propagation in Dynamical · PDF file 8/10/2012  · Linear systems: Kalman lter/smoother (Kalman, 1959) Nonlinear systems: Approximate inference Extended Kalman Filter/Smoother
Documents ##### Factor Analysed Hidden Markov Models for Conditionally ... · PDF file Key-words: Dynamic Factor Analysis, GQARCH Processes, HMM, EM Algorithm, Switching Kalman ˝lter, Viterbi Approximation,
Documents ##### Introduction to Local Level Model and Kalman Filter · PDF file Kalman Filter I The Kalman lter calculates the mean and variance of the unobserved state, given the observations. I
Documents ##### An Introduction to Particle Filtering - Lancaster · PDF file2 The Kalman Filter The Kalman lter is a special class of particle lter, also known as a linear Gaussian state space model,
Documents ##### Maximum likelihood estimation of time series models: the Kalman �lter and beyond
Documents ##### Extended Kalman Filter Based Pose Estimation Using ... khwong/internal_report_khw_irep_  · PDF file model. In this case, using Extended Kalman Fi lter (EKF) is necessary. In fact
Documents ##### Kalman Filtering in R - COnnecting · PDF file · 2017-05-05The Kalman lter equations, with ... Direct transcription of the equations making up the Kalman lter into computer code
Documents ##### Kalman Filtering in R - COnnecting REpositories · PDF file 2 Kalman Filtering in R 2. Kalman lter algorithms We shall consider a fairly general state-space model speci cation, su
Documents Documents