Ch.

13 Kalman Filters

Introduction
In 1960, Rudolf Kalman developed a way to solve some of the practical difficulties that arise when trying to apply Weiner filters. There are D-T and C-T versions of the Kalman Filter we will only consider the D-T version. KF initially arose in the The Kalman filter is widely used in: Control Systems Navigation Systems Tracking Systems
field of control systems in order to make a system do what you want, you must know what it is doing now

It is less widely used in signal processing applications

The Three Keys to Leading to the Kalman Filter

Wiener Filter: LMMSE of a Signal (i.e., a Varying Parameter) Sequential LMMSE: Sequentially Estimate a Fixed Parameter State-Space Models: Dynamical Models for Varying Parameters
Kalman Filter: Sequential LMMSE Estimation for a timevarying parameter vector but the time variation is constrained to follow a state-space dynamical model.

Aside: There are many ways to mathematically model dynamical systems Differential/Difference Equations Convolution Integral/Summation Transfer Function via Laplace/Z transforms State-Space Model
3

13.3 State-Variable Dynamical Models

System State: the collection of variables needed to know how to determine how the system will exist at some future time (in the absence of an input) For an RLC circuit you need to know all of its current capacitor voltages and all of its current inductor currents

Motivational Example: Constant Velocity Aircraft in 2-D

rx (t ) ry (t ) s( t ) = v x (t ) v y ( t )

A/C positions (m) A/C velocities (m/s)

For the constant velocity model we would constrain vx(t) & vy(t) to be constants Vx & Vy.

If we know s(to) and there is no input we know how the A/C behaves for all future times: rx(to + ) = Vx + rx(to) rx(to + ) = rx(to) + Vx ry(to + ) = ry(to) + Vy
4

D-T State Model for Constant Velocity A/C

Because measurements are often taken at discrete times we often need D-T models for what are otherwise C-T systems
(This is the same as using a difference equation to approximate a differential equation)

If every increment of n corresponds to a duration of sec and there is no driving force then we can write a D-T State Model as:

s [n ] = As [n 1]
0 1 0 0 1 0 A= 0 0 1 0 0 0 0 1 State Transition Matrix

rx[n] = rx[n-1] + vx[n-1] ry [n] = ry[n-1] + vy[n-1] vx[n] = vx[n-1] vy[n] = vy[n-1] We can include the effect of a vector input:

s [n ] = As [n 1] + Bu[n ]
Input could be deterministic and/or random. Matrix B combines inputs & distributes them to states.
5

Thm 13.1 Vector Gauss-Markov Model

This theorem characterizes the probability model for a specific state-space model with Gaussian Inputs Linear State Model: s [n ] = As [n 1] + Bu[n ]
p1 pp known pr known r1

Dont confuse with the G-M Thm. of Ch. 6

n0

s[n]: A: B: u[n]: s[-1]:

state vector is a vector Gauss-Markov process state transition matrix; assumed |i| < 1 for stability input matrix eigenvalues driving noise is vector WGN w/ zero mean initial state ~ N(s,Cs) and independent of u[n]
u[n] ~ N(0,Q) E{u[n] uT[m]} = 0, n m
6

Theorem: s[n] for n 0 is Gaussian with the following characteristics diverges if e-values Mean of state vector is E{s[n ]} = A n +1 s Covariance between state vectors at m and n is
have |i| 1

for m n : Cs [m, n ] = E [s[m] E{s[m]}][s[n ] E{s[n ]}]T =A

m +1

Cs A

n +1 T

k =m n

A k BQBT ( A n-m+ k )T

for m < n : Cs [m, n ] = CT [n, m] s

State Process is Not WSS!

Covariance Matrix: C[n] = Cs[n,n] Propagation of Mean & Covariance:

(this is just notation)

E{s[n ]} = AE{s[n 1]} C[n ] = AC[n 1]A T + BQBT

7

Proof: (only for the scalar case: p = 1)

differs a bit from (13.1) etc.

For the scalar case the model is: s[n] = a s[n-1] + b u[n]
s[0] = as[ 1] + bu[0]
s[1] = as[0] + bu[1] = a 2 s[ 1] + abu[0] + bu[1]
s[2] = as[1] + bu[2] = a 3 s[ 1] + a 2 bu[0] + abu[1] + bu[2]

n0

Now we can just iterate this model and surmise its general form:

!
Now easy to find the mean:
E {s[n ]} = a
n +1

s[n ] = a
=0

n +1

s[ 1] +

E {$" } %s[ # + " 1]

=s

k =0

b %"n k ]} E {u[$" #
z.i. response exponential

k =0

a k bu[n k ]
z.s. response convolution

= a n +1 s

as claimed!

Covariance between s[m] and s[n] is:

C s [m, n ] = E [ s[m] a m +1 s ][ s[n ] a n +1 s ]T
m +1 m

{ = E{(a

}
)
Must use different dummy variables!!

[ s[m] s ] + a k bu[m k ]
k =0 n

Cross-terms will be zero why?

(a n+1[s[n] s ] + a l bu[n l ])}

l =0 2 a m +1a n +1 s

+ a k b E{u[m k ]u[n l ]}ba l %"" $""" " #

k = 0l = 0
2 = u [ l ( n m + k )]

m m

For m n:

2 C s [m, n ] = a m +1a n +1 s

k =mn

2 a k b u ba n m + k

For m < n: C s [m, n ] = C s [n, m]

9

For mean & cov. propagation: from s[n] = a s[n 1] + b u[n]

E{s[n ]} = aE{s[n 1]} + b E{u[n ]} %""" $"""" " # %$# " "
propagates as in theorem =0

var{s[n ]} = E ( s[n ] E{s[n ]}) 2

{ } = E { ( as[n 1] + bu[n ] aE{s[n 1]}) } = a E { ( s[n 1] E{s[n 1]}) }a + b E { u [n ]}b %"""" $""""" " # % $" " #
2 2 2 = var{s[ n 1]}
2 = u

which propagates as in theorem

< End of Proof >

So we now have: Random Dynamical Model (A State Model) Statistical Characterization of it

10

Random Model for Constant Velocity A/C

rx [n ] 1 ry [n ] 0 = s[n ] = v x [n ] 0 v y [n ] 0 0 1 0 0 0 1 0 0 rx [n 1] 0 ry [n 1] 0 + 0 v x [n 1] u x [n ] 1 v y [n 1] u y [n ]

Deterministic Propagation of Constant-Velocity Random Perturbation of Constant Velocities

0 0 cov{u[n ]} = 0 0 0 0 0 0 0 0
2 u

0 0 0 2 u

11

Ex. Set of Constant-Velocity A/C Trajectories

14000

Red Line is Non-Random Constant Velocity Trajectory

12000

10000

Y position (m)

8000

Acceleration of (5 m/s)/1s = 5m/s2

= 1 sec

6000

4000

u = 5 m/s
rx [ 1] = ry [ 1] = 0 m v x [ 1] = v y [ 1] = 100 m/s

2000

2000

4000

6000

8000 10000 X position (m)

12000 14000 16000

18000

12

Observation Model
So we have a random state-variable model for the dynamics of the signal ( the signal is often some true A/C trajectory) We need to have some observations (i.e., measurements) of the signal
In Navigation Systems inertial sensors make noisy measurements at intervals of time In Tracking Systems sensing systems make noisy measurements (e.g., range and angles) at intervals of time

Linear Observation Model:

Measured observation vector at each time

x[n ] = H[n ] s[n ] + w[n ]

Vector Noise Process

Observation Matrix can change w/ time

allows multiple measurements at each time

State Vector Process being observed

13

The Estimation Problem

Observe a Sequence of Observation Vectors {x[0], x[1], x[n]} Compute an Estimate of the State Vector s[n]
s[n | n ]
using observation up to n estimate state at n

Notation: s[n | m ] = Estimate of s[n] using {x[0], x[1], x[m]}

Want Recursive Solution: Given: s[n | n ] and a new observation vector x[n + 1] Find: s[n + 1 | n + 1]
Three Cases of Interest: Scalar State Scalar Observation Vector State Scalar Observation Vector State Vector Observation

14