Slide 1
Slide 1 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 1/12
A New Approach to Linear Filtering and Prediction
Problems
a.k.a. Kalman Filters
Papers We Love NYC
Elizabeth Ramirez
I
n [
1
]
: %
m
a
t
p
l
o
t
l
i
b i
n
l
i
n
e
i
m
p
o
r
t n
u
m
p
y
f
r
o
m n
u
m
p
y
.
r
a
n
d
o
m i
m
p
o
r
t r
a
n
d
n
i
m
p
o
r
t m
a
t
p
l
o
t
l
i
b
.
p
y
p
l
o
t a
s p
l
t
Intro
Original paper: [http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
(http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf)]
Slide 2
Slide 2 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 2/12
What is a Kalman Filter
Prediction-correction technique
Calculate recursively system state at time given previous state and new information
Kalman Filters for Rocket Science
Used for Apollo Space Program of NASA in early 1960's
Transcription of the original code available at [http://www.ibiblio.org/apollo/ (http://www.ibiblio.org/apollo/)]
Implemented in AGC4 assembly language.
# C
o
p
y
r
i
g
h
t
: P
u
b
l
i
c d
o
m
a
i
n
.
# F
i
l
e
n
a
m
e
: K
A
L
M
A
N
_
F
I
L
T
E
R
.
a
g
c
# P
u
r
p
o
s
e
: A s
e
c
t
i
o
n o
f L
u
m
i
n
a
r
y 1
C
, r
e
v
i
s
i
o
n 1
3
1
.
# I
t i
s p
a
r
t o
f t
h
e s
o
u
r
c
e c
o
d
e f
o
r t
h
e L
u
n
a
r M
o
d
u
l
e
'
s (
L
M
)
# A
p
o
l
l
o G
u
i
d
a
n
c
e C
o
m
p
u
t
e
r (
A
G
C
) f
o
r A
p
o
l
l
o 1
3 a
n
d A
p
o
l
l
o 1
4
.
# T
h
i
s f
i
l
e i
s i
n
t
e
n
d
e
d t
o b
e a f
a
i
t
h
f
u
l t
r
a
n
s
c
r
i
p
t
i
o
n
, e
x
c
e
p
t
# t
h
a
t t
h
e c
o
d
e f
o
r
m
a
t h
a
s b
e
e
n c
h
a
n
g
e
d t
o c
o
n
f
o
r
m t
o t
h
e
# r
e
q
u
i
r
e
m
e
n
t
s o
f t
h
e y
a
Y
U
L a
s
s
e
m
b
l
e
r r
a
t
h
e
r t
h
a
n t
h
e
# o
r
i
g
i
n
a
l Y
U
L a
s
s
e
m
b
l
e
r
.
# R
e
f
e
r
e
n
c
e
: p
p
. 1
4
6
7
-
1
4
6
8 o
f 1
7
2
9
.
p
d
f
.
# C
o
n
t
a
c
t
: R
o
n B
u
r
k
e
y <
i
n
f
o
@
s
a
n
d
r
o
i
d
.
o
r
g
>
.
# W
e
b
s
i
t
e
: w
w
w
.
i
b
i
b
l
i
o
.
o
r
g
/
a
p
o
l
l
o
/
i
n
d
e
x
.
h
t
m
l
# M
o
d h
i
s
t
o
r
y
: 0
6
/
0
8
/
0
3 R
S
B
. B
e
g
a
n t
r
a
n
s
c
r
i
b
i
n
g
.
# 0
5
/
1
4
/
0
5 R
S
B C
o
r
r
e
c
t
e
d w
e
b
s
i
t
e r
e
f
e
r
e
n
c
e a
b
o
v
e
Background
Random processes (Appendix)
Dynamic systems
Vector spaces
Least Squares and Normal Equation
Vector Spaces
a.k.a. Linear Spaces. A space of vectors with a metric: norm, notion of distance, often represented by
and the following properties:
u
k
t
k
u
k
−1
b
k
||
⋅ ||
f , g
∈
→ f + g = h
∈
λ ∈ ℝ, f
∈
→ λf
∈
Slide 3
Slide 3 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 3/12
Least Squares
Motivation
Wiener Filter Problems:
Impulse response
Numerical determination
Least Squares
Linear System Au = b A = m × n u, b are n-vectors
If A is square: m = n
→ u = b
A
−1
But if A is not square: m > n
System is overdetermined
Example: 100 points that
fit Cx + D
Solution: Find best estimate for state u that minimizes:
E = b
− A
∣
∣ u
̂
∣
∣2
Solve for (an estimate) to minimize E
u
̂
Normal Equation: A = b
A
T
u
̂
A
T
=
A
T
n×m
A
m×n
A
A
T
n×n
= ( A b
u
̂
A
T
)
−1
A
T
Slide 4
Slide 4 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 4/12
Situation
Theorem 1
Given a signal: y(t) = (t) + (t)
x
1
x
2
(t) : true signal, (t) : noise
x
1
x
2
y( )?
t
1
If < t : Smoothing
t
1
If = t : Filtering
t
1
If > t : Prediction
t
1
Statistical Estimate ( |t)
X
1
t
1
Estimation Error
ϵ = ( )
− ( |t)
x
1
t
1
X
1
t
1
Loss Function L(
ϵ) :
L(0) = 0
L( )
≥ L( )
≥ 0 when
≥ ≥ 0
ϵ
2
ϵ
1
ϵ
2
ϵ
1
L(
ϵ) = L(
−ϵ)
Minimize E{L[ ( )
− ( )]|y( )
…y(t)}
x
1
t
1
X
1
t
1
t
0
Slide 5
Slide 5 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 5/12
Theorem 1-a
Problem Summary
Orthogonal Projections
If L(
ϵ) = , Theorem 1 is true without assumptions about symmetry and convexity around the mean.
ϵ2
Given a vector-valued random process x(t)
and observed random variables y( )
…y(t)
t
0
Find an estimate X( ) that minimizes the expected loss E{L||x( )
− X( )||}
t
1
t
1
t
1
Vector Space (t) = y(i)
∑
i=t
0
t
a
i
Given u, v
∈ (t) orthogonal if E[u
⋅ v] = 0
Orthonormal basis in (t) = . . . , such that
e
t
0
e
t
=
x
¯
∑
i=t
0
t
a
i
e
i
x = +
x
¯ x
̃
Slide 6
Slide 6 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 6/12
From functional analysis, we know that our estimate X1(t1) might or might not live in this vector space Y(t). We
would really like our solution to live here. If the vector space is too small, we want to minimize the distance
between my estimate and the vector space.
Given this, and given we can determine the coefficients of the linear combination, any random variable x, not
necessarily in Y(t) can be uniquely decomposed into two parts: a part in Y(t) and a part orthogonal to Y(t)
(orthogonal to every vector in Y(t)).
See equation 10. The orthogonal projection of x in Y(t): x bar is that vector in Y(t) that minimizes the quadratic
loss. This is the key part.
Theorem 2
Recursive Least Squares
Let {x(t)} and {y(t)} be random processes with zero mean E[x(t)] = E[y(t)] = 0
Observe y( ). . . y(t)
t
0
If either the random processes are gaussian or the loss function is restricted to be L(
ϵ) =
ϵ2
then the optimal estimate of x(t1|t) is given by the orthogonal projection of x( ) on (t)
x
¯ t
1
: Average of , ,
…,
u
99
^
b
1
b
2
b
99
→
b
100
u
100
^
= +
u
100
^
99
100
u
99
^
1
100
b
100
= + (
− )
u
99
^
1
100
b
100
u
99
^
− : innovation : gain factor
b
100
u
99
^
1
100
Slide 7
Slide 7 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 7/12
Recursive Least Squares
Generalizing the Normal Equation:
Right-hand side
Recursive Least Squares
[ ]
[ ] =
[ ]
→
A
old
A
new
u
b
old
b
new
u
new
^
= [ ]
A
T
AT
old
AT
new
A = [ ]
[ ]
A
T
AT
old
AT
new
AT
old
AT
new
= + = (known) + (new)
AT
old
A
old
AT
new
A
new
b = [ ]
[ ]
A
T
AT
old
AT
new
bT
old
bT
new
= +
AT
old
b
old
AT
new
b
new
= +
AT
old
A
old
u
old
^
AT
new
b
new
= + ( A (
− )
u
new
^
u
old
^
A
T
)
−1
AT
new
b
new
A
new
b
old
( A : gain matrix, K (as in Kalman, duh)
A
T
)
−1
AT
new
[ ] =
⎡
⎣
⎢
⎢
⎢
⎢
1
1
⋮
1
⎤
⎦
⎥
⎥
⎥
⎥
u
⎡
⎣
⎢
⎢
⎢
⎢
b
1
b
2
⋮
b
99
⎤
⎦
⎥
⎥
⎥
⎥
u = =
→ = [ ]
b100
bnew
Anew 1
= + (
− )
u
100
^
u
99
^
1
100
b
100
u
99
^
Slide 8
Slide 8 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 8/12
Covariance Matrix
When errors are independent:
Weighted Normal Equation
The Kalman Filter
= = E[ ] = 0
σ
ij
σ
ji
e
i
e
j
= E(e ) =
Σ
e
e
T
⎡
⎣
⎢
⎢
⎢
⎢
⎢
σ2
1
σ2
2
⋱
σ2
k
⎤
⎦
⎥
⎥
⎥
⎥
⎥
C = : Weighting Matrix
Σ
−1
e
CA = Cb
A
T
u
̂
A
T
= ( CA Cb = Lb
u
̂
A
T
)
−1
A
T
Time varying least squares problem: Estimate at each time
u
k
^
t
k
= L + K
u
new
^
u
old
^
b
new
= + K(
− )
u
new
^
u
old
^
b
new
A
new
u
old
^
innovation
−
b
new
A
new
u
old
^
covariance matrix P = ( CA
A
T
)
−1
Slide 9
Slide 9 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 9/12
Algorithm
Prediction
Correction
F is called the state transition matrix. It establishes how the state vector changes from one time step to another.
Q is the covariance matrix of system error
Implementation
Predicted state and covariance (before the measurement)
Corrected state and covariance (after the measurement)
Innovation
Filter gain
Prediction / Correction
Previous state vector
Predicted state vector
Matrix of observation equations
Vector of observations
Predicted covariance matrix
Process noise matrix
Observations noise matrix
= F +
u
k|k
−1
^
u
k
−1|k
−1
^ ϵ
i
= A + Q
P
k|k
−1
P
k
−1|k
−1
A
T
F : state transition matrix
Q : covariance matrix of random excitation
= (A + R
K
k
P
k|k
−1
A
T
P
k|k
−1
A
T
)
−1
= + (
− A )
u
k|k
^
u
k|k
−1
^
K
k
b
k
u
k|k
−1
^
= (I
− A)
P
k|k
K
k
P
k|k
−1
u
u
̂
A
b
P
Q
R
Slide 10
Slide 10 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 10/12
I
n [
2
]
: d
e
f p
r
e
d
i
c
t
(
u
, P
, F
, Q
)
:
u = n
u
m
p
y
.
d
o
t
(
F
, u
)
P = n
u
m
p
y
.
d
o
t
(
F
, n
u
m
p
y
.
d
o
t
(
P
, F
.
T
)
) + Q
r
e
t
u
r
n u
, P
I
n [
3
]
: d
e
f c
o
r
r
e
c
t
(
u
, A
, b
, P
, Q
, R
)
:
C = n
u
m
p
y
.
d
o
t
(
A
, n
u
m
p
y
.
d
o
t
(
P
, A
.
T
)
) + R
K = n
u
m
p
y
.
d
o
t
(
P
, n
u
m
p
y
.
d
o
t
(
A
.
T
, n
u
m
p
y
.
l
i
n
a
l
g
.
i
n
v
(
C
)
)
)
u = u + n
u
m
p
y
.
d
o
t
(
K
, (
b - n
u
m
p
y
.
d
o
t
(
A
, u
)
)
)
P = P - n
u
m
p
y
.
d
o
t
(
K
, n
u
m
p
y
.
d
o
t
(
C
, K
.
T
)
)
r
e
t
u
r
n u
, P
I
n [
4
]
: # T
e
s
t d
a
t
a
d
t = 0
.
1
A = n
u
m
p
y
.
a
r
r
a
y
(
[
[
1
, 0
]
, [
0
, 1
]
]
)
u = n
u
m
p
y
.
z
e
r
o
s
(
(
2
, 1
)
)
# R
a
n
d
o
m i
n
i
t
i
a
l m
e
a
s
u
r
e
m
e
n
t c
e
n
t
e
r
e
d a
t s
t
a
t
e v
a
l
u
e
b = n
u
m
p
y
.
a
r
r
a
y
(
[
[
u
[
0
, 0
] + r
a
n
d
n
(
1
)
[
0
]
]
, [
u
[
1
, 0
] + r
a
n
d
n
(
1
)
[
0
]
]
]
)
P = n
u
m
p
y
.
d
i
a
g
(
(
0
.
0
1
, 0
.
0
1
)
)
F = n
u
m
p
y
.
a
r
r
a
y
(
[
[
1
.
0
, d
t
]
, [
0
.
0
, 1
.
0
]
]
)
# U
n
i
t v
a
r
i
a
n
c
e f
o
r t
h
e s
a
k
e o
f s
i
m
p
l
i
c
i
t
y
Q = n
u
m
p
y
.
e
y
e
(
u
.
s
h
a
p
e
[
0
]
)
R = n
u
m
p
y
.
e
y
e
(
b
.
s
h
a
p
e
[
0
]
)
I
n [
5
]
: N = 5
0
0
p
r
e
d
i
c
t
i
o
n
s
, c
o
r
r
e
c
t
i
o
n
s
, m
e
a
s
u
r
e
m
e
n
t
s = [
]
, [
]
, [
]
f
o
r k i
n n
u
m
p
y
.
a
r
a
n
g
e
(
0
, N
)
:
u
, P = p
r
e
d
i
c
t
(
u
, P
, F
, Q
)
p
r
e
d
i
c
t
i
o
n
s
.
a
p
p
e
n
d
(
u
)
u
, P = c
o
r
r
e
c
t
(
u
, A
, b
, P
, Q
, R
)
c
o
r
r
e
c
t
i
o
n
s
.
a
p
p
e
n
d
(
u
)
m
e
a
s
u
r
e
m
e
n
t
s
.
a
p
p
e
n
d
(
b
)
b = n
u
m
p
y
.
a
r
r
a
y
(
[
[
u
[
0
, 0
] + r
a
n
d
n
(
1
)
[
0
]
]
,
[
u
[
1
, 0
] + r
a
n
d
n
(
1
)
[
0
]
]
]
)
p
r
i
n
t '
p
r
e
d
i
c
t
e
d f
i
n
a
l e
s
t
i
m
a
t
e
: %
f
' % p
r
e
d
i
c
t
i
o
n
s
[
-
1
]
[
0
]
p
r
i
n
t '
c
o
r
r
e
c
t
e
d f
i
n
a
l e
s
t
i
m
a
t
e
: %
f
' % c
o
r
r
e
c
t
i
o
n
s
[
-
1
]
[
0
]
p
r
i
n
t '
m
e
a
s
u
r
e
d s
t
a
t
e
: %
f
' % m
e
a
s
u
r
e
m
e
n
t
s
[
-
1
]
[
0
]
p
r
e
d
i
c
t
e
d f
i
n
a
l e
s
t
i
m
a
t
e
: 1
7
7
.
5
2
9
1
1
3
c
o
r
r
e
c
t
e
d f
i
n
a
l e
s
t
i
m
a
t
e
: 1
7
6
.
6
0
2
6
2
7
m
e
a
s
u
r
e
d s
t
a
t
e
: 1
7
6
.
0
0
7
3
6
1
Slide 11
Slide 11 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 11/12
I
n [
6
]
: t = n
u
m
p
y
.
a
r
a
n
g
e
(
5
0
, 1
0
0
)
f
i
g = p
l
t
.
f
i
g
u
r
e
(
f
i
g
s
i
z
e
=
(
1
5
,
1
5
)
)
a
x
e
s = f
i
g
.
a
d
d
_
s
u
b
p
l
o
t
(
2
, 2
, 1
)
a
x
e
s
.
s
e
t
_
t
i
t
l
e
(
"
K
a
l
m
a
n F
i
l
t
e
r
"
)
a
x
e
s
.
p
l
o
t
(
t
, n
u
m
p
y
.
a
r
r
a
y
(
p
r
e
d
i
c
t
i
o
n
s
)
[
5
0
:
1
0
0
, 0
]
, '
o
'
, l
a
b
e
l
=
'
p
r
e
d
i
c
t
i
o
n
'
)
a
x
e
s
.
p
l
o
t
(
t
, n
u
m
p
y
.
a
r
r
a
y
(
c
o
r
r
e
c
t
i
o
n
s
)
[
5
0
:
1
0
0
, 0
]
, '
x
'
, l
a
b
e
l
=
'
c
o
r
r
e
c
t
i
o
n
'
)
a
x
e
s
.
p
l
o
t
(
t
, n
u
m
p
y
.
a
r
r
a
y
(
m
e
a
s
u
r
e
m
e
n
t
s
)
[
5
0
:
1
0
0
, 0
]
, '
^
'
, l
a
b
e
l
=
'
m
e
a
s
u
r
e
m
e
n
t
'
)
p
l
t
.
l
e
g
e
n
d
(
)
p
l
t
.
s
h
o
w
(
)
Conclusions
Kalman Filter is a generalized least squares problem for dynamical systems.
Computationally more efficient
Strang, Gilbert. Computational Science and Engineering
Slide 12
Slide 12 text
10/8/2016 pwlnyc_kalman
http://localhost:8888/nbconvert/html/pwlnyc_kalman.ipynb?download=false 12/12
Thank you!
[email protected]
@eramirem