KalmanIntroduction just for introduction people

Kalman’s Beautiful Filter
(an introduction)
George Kantor
presented to
Sensor Based Planning Lab
Carnegie Mellon University
December 8, 2000

Kalman Filter Introduction Carnegie Mellon University
December 8, 2000
What does a Kalman Filter do, anyway?
x k F k x k G k u k v k
y k H k x k w k
( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
   
 
1
x k n
u k m
y k p
F k G k H k
v k w k
Q k R k
( )
( )
( )
( ), ( ), ( )
( ), ( )
( ), ( )
is the -dimensional state vector (unknown)
is the -dimensional input vector (known)
is the -dimensional output vector (known, measured)
are appropriately dimensioned system matrices (known)
are zero - mean, white Gaussian noise with (known)
covariance matrices
Given the linear dynamical system:
the Kalman Filter is a recursion that provides the
“best” estimate of the state vector x.

December 8, 2000
What’s so great about that?
• noise smoothing (improve noisy measurements)
• state estimation (for state feedback)
• recursive (computes next estimate using only most
recent measurement)
y k H k x k w k
( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
   
 
1

December 8, 2000
How does it work?
y k H k x k w k
( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
   
 
1
)
|
1
(
ˆ
)
(
)
(
ˆ
)
(
)
(
)
|
(
ˆ
)
(
)
|
1
(
ˆ
k
k
x
k
H
k
y
k
u
k
G
k
k
x
k
F
k
k
x





1. prediction based on last estimate:
2. calculate correction based on prediction and current measurement:
 
)
|
1
(
ˆ
),
1
( k
k
x
k
y
f
x 



3. update prediction: x
k
k
x
k
k
x 




 )
|
1
(
ˆ
)
1
|
1
(
ˆ

December 8, 2000
Finding the correction (no noise!)
.
of
estimate
best"
"
the
is
ˆ
ˆ
that
so
find
,
output
and
ˆ
prediction
Given
x
x
x
x
x
y
x 


 

Hx
y 
 
y
Hx
x 

 |

x̂

  
1


 T
T
HH
H
x

December 8, 2000
A Geometric Interpretation
 
y
Hx
x 

 |

x̂

x


December 8, 2000
A Simple State Observer
)
(
)
(
)
(
)
(
)
(
)
1
(
k
Hx
k
y
k
v
k
Gu
k
Fx
k
x





)
(
)
|
(
ˆ
)
|
1
(
ˆ k
Gu
k
k
x
F
k
k
x 


   
)
|
1
(
ˆ
)
1
(
1
k
k
x
H
k
y
HH
H
x T
T






x
k
k
x
k
k
x 




 )
|
1
(
ˆ
)
1
|
1
(
ˆ
System:
1. prediction:
2. compute correction:
3. update:
Observer:

December 8, 2000
where is the covariance matrix
Estimating a distribution for x
Our estimate of x is not exact!
We can do better by estimating a joint Gaussian distribution p(x).
 
)
ˆ
(
)
ˆ
(
2
1
2
/
1
2
/
1
)
2
(
1
)
(
x
x
P
x
x
n
T
e
P
x
p


 


 
T
x
x
x
x
E
P )
ˆ
)(
ˆ
( 



December 8, 2000
Finding the correction (geometric intuition)
.
of
estimate
probable)
most
(i.e.
best"
"
the
is
ˆ
ˆ
that
so
find
,
output
and
,
covariance
,
ˆ
prediction
Given
x
x
x
x
x
y
P
x






 
y
Hx
x 

 |

x̂

x

x
P
x
x
x
x
x
T








1
minimizes
2.
ˆ
ˆ
satisfies
1.
:
that
one
the
is
probable
most
The
 
)
ˆ
(
)
ˆ
(
2
1
2
/
1
2
/
1
)
2
(
1
)
(
x
x
P
x
x
n
T
e
P
x
p


 



December 8, 2000
A new kind of distance
:
be
to
on
product
inner
new
a
define
we
Suppose n
R
)
product
inner
old
the
replaces
(this
, 2
1
2
1
1
2
1 x
x
x
P
x
x
x T
T 

x
P
x
x
x
x T 1
2
,
norm
new
a
define
can
Then we 


.
to
orthogonal
is
so
,
onto
ˆ
of
projection
orthogonal
the
is
minimizes
that
in
ˆ
The




 
x
x
x
x
)
(
T
in
for
0
, H
null
x 



 

)
(
iff
0
, 1 T
T
PH
column
x
x
P
x 




 



December 8, 2000
Finding the correction (for real this time!)
ˆ
in
linear
is
that
Assuming 


 x
H
y
x 

K
PH
x T










 
 x
H
y
x
H
x
x
H
y ˆ
)
ˆ
(
condition
The
:
yields
on
Substituti

 K
HPH
x
H T



 1


 T
HPH
K
 1




 T
T
HPH
PH
x

December 8, 2000
A Better State Observer
We can create a better state observer following the same 3. steps, but now we
must also estimate the covariance matrix P.
Step 1: Prediction
We start with x(k|k) and P(k|k)
)
(
)
|
(
ˆ
)
|
1
(
ˆ k
Gu
k
k
x
F
k
k
x 


What about P? From the definition:
 
T
k
k
x
k
x
k
k
x
k
x
E
k
k
P ))
|
(
ˆ
)
(
))(
|
(
ˆ
)
(
(
)
|
( 


and
 
T
k
k
x
k
x
k
k
x
k
x
E
k
k
P ))
|
1
(
ˆ
)
1
(
))(
|
1
(
ˆ
)
1
(
(
)
|
1
( 








December 8, 2000
Continuing Step 1
To make life a little easier, lets shift notation slightly:
 
T
k
k
k
k
k x
x
x
x
E
P )
ˆ
)(
ˆ
( 1
1
1
1
1







 


  
 
T
k
k
k
k
k
k
k
k
k
k Gu
x
F
v
Gu
Fx
Gu
x
F
v
Gu
Fx
E )
ˆ
(
)
ˆ
( 








 
   
 
 
T
k
k
k
k
k
k v
x
x
F
v
x
x
F
E 



 ˆ
ˆ
    
 
T
k
T
k
k
T
T
k
k
k
k k
k
v
v
v
x
x
F
F
x
x
x
x
F
E 




 ˆ
2
ˆ
ˆ
  
   
T
k
T
T
k
k
k
k k
v
v
E
F
x
x
x
x
FE 


 ˆ
ˆ
Q
F
FP T
k 

Q
F
k
k
FP
k
k
P T


 )
|
(
)
|
1
(

December 8, 2000
Step 2: Computing the correction
).
|
1
(
and
)
|
1
(
ˆ
get
we
1
step
From k
k
P
k
k
x 

:
compute
to
these
use
we
Now x

   
)
|
1
(
ˆ
)
1
(
)
|
1
(
)
|
1
(
1
k
k
x
H
k
y
H
k
k
HP
H
k
k
P
x T








For ease of notation, define W so that

W
x 


December 8, 2000
Step 3: Update
)
|
1
(
ˆ
)
1
|
1
(
ˆ 
W
k
k
x
k
k
x 




 
T
k
k
k
k
k x
x
x
x
E
P )
ˆ
)(
ˆ
( 1
1
1
1
1 



 


 
T
k
k
k
k W
x
x
W
x
x
E )
ˆ
)(
ˆ
( 1
1
1
1 
 



 





(just take my word for it…)
T
T
W
H
k
k
WHP
k
k
P
k
k
P )
|
1
(
)
|
1
(
)
1
|
1
( 






December 8, 2000
Just take my word for it…
 
T
k
k
k
k
k x
x
x
x
E
P )
ˆ
)(
ˆ
( 1
1
1
1
1 



 


 
T
k
k
k
k W
x
x
W
x
x
E )
ˆ
)(
ˆ
( 1
1
1
1 
 



 





   




 



 





T
k
k
k
k W
x
x
W
x
x
E 
 )
ˆ
(
)
ˆ
( 1
1
1
1
 
 
T
T
k
k
T
k
k
k
k W
W
x
x
W
x
x
x
x
E 

 




 







 )
ˆ
(
2
)
ˆ
)(
ˆ
( 1
1
1
1
1
1
 
T
T
T
k
k
k
k
T
k
k
k
k
k W
H
x
x
x
x
WH
x
x
x
x
WH
E
P )
ˆ
)(
ˆ
(
)
ˆ
)(
ˆ
(
2 1
1
1
1
1
1
1
1
1













 







T
T
k
k
k W
H
WHP
WHP
P 




 

 1
1
1 2
  T
T
k
k
T
k
T
k
k W
H
WHP
HP
H
HP
H
P
P 









 

 1
1
1
1
1
1 2
     T
T
k
k
T
k
T
k
T
k
T
k
k W
H
WHP
HP
H
HP
H
HP
H
HP
H
P
P 














 

 1
1
1
1
1
1
1
1
1 2
T
T
k
T
T
k
k W
H
WHP
W
H
WHP
P 




 

 1
1
1 2

December 8, 2000
Better State Observer Summary
)
(
)
(
)
(
)
(
)
(
)
1
(
k
Hx
k
y
k
v
k
Gu
k
Fx
k
x





System:
1. Predict
2. Correction
3. Update
)
(
)
|
(
ˆ
)
|
1
(
ˆ k
Gu
k
k
x
F
k
k
x 


Q
F
k
k
FP
k
k
P T


 )
|
(
)
|
1
(
 
)
|
1
(
)
|
1
(
1



 T
H
k
k
HP
H
k
k
P
W
 
)
|
1
(
ˆ
)
1
( k
k
x
H
k
y
W
x 




)
|
1
(
ˆ
)
1
|
1
(
ˆ 
W
k
k
x
k
k
x 




T
T
W
H
k
k
WHP
k
k
P
k
k
P )
|
1
(
)
|
1
(
)
1
|
1
( 





Observer

December 8, 2000
Finding the correction (with output noise)
w
Hx
y 

 
y
Hx
x 

 |

x̂

Since you don’t have a hyperplane to
aim for, you can’t solve this with algebra!
You have to solve an optimization problem.
That’s exactly what Kalman did!
Here’s his answer:
   





 x
H
y
R
HPH
PH
x T
T
ˆ
1

December 8, 2000
LTI Kalman Filter Summary
)
(
)
(
)
(
)
(
)
(
)
(
)
1
(
k
w
k
Hx
k
y
k
v
k
Gu
k
Fx
k
x






System:
1. Predict
2. Correction
3. Update
)
(
)
|
(
ˆ
)
|
1
(
ˆ k
Gu
k
k
x
F
k
k
x 


Q
F
k
k
FP
k
k
P T


 )
|
(
)
|
1
(
)
|
1
(
ˆ
)
1
|
1
(
ˆ 
W
k
k
x
k
k
x 




T
WSW
k
k
P
k
k
P 



 )
|
1
(
)
1
|
1
(
Kalman
Filter
)
|
1
( 1


 HS
k
k
P
W
 
)
|
1
(
ˆ
)
1
( k
k
x
H
k
y
W
x 




R
)
|
1
( 

 T
H
k
k
HP
S

KalmanIntroduction just for introduction people

More Related Content

Similar to KalmanIntroduction just for introduction people

More from vishalkumarpandey12

Recently uploaded

KalmanIntroduction just for introduction people