Silvio-Simani-Kalman-filter-theory_4p

“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Kalman filtering (KF): Theory and Applications
Dr. Silvio Simani
Dept. of Engineering, University of Ferrara Engineering, V. Saragat, 1. I-44100, Ferrara, Italy. Saragat, 1. Italy. Ph.: +39 0532 97 4844, fax: +39 0532 97 4870 h.: 4844, E-MAIL: ssimani@ing.unife.it ssimani@ ing. unife.it HOME PAGE: http://www.ing.unife.it/simani/ http://www.ing.unife.it/simani/
Kalman filtering (KF): Theory
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
2
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
3
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
4


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
5
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
6
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
One makes a measurement
Another one makes a measurement
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
7
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
8


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
9
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
10
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
11
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
12


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
13
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
14
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
15
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
16


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
17
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
18
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
19
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
20


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Expectations
• Let x be a random variable. • The expected value E[x] is the mean:
N 1
Variance and Covariance
• The variance is E[ (x-E[x])2 ] 1 N 2 2 σ = E[(x − x ) ] = ∑ (x i − x ) 2 N 1 • Covariance matrix is E[ (x-E[x])(x-E[x])T ]
1 E[ x] = x = N
∑x
i
– The probability-weighted mean of all possible values. The sample mean approaches it.
• Expected value of a vector x is by component.
1 Cij = N
∑(x
k =1
N
ik
− x i )(x jk − x j )
E[x] = x = [x1,Lx n ]T
Summer School 2006 – Bologna, Italy.
– Divide by N−1 to make the sample variance an unbiased estimator for the population variance.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
21
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
22
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Covariance Matrix
• Along the diagonal, Cii are variances. • Off-diagonal Cij are essentially correlations.
⎡C1,1 = σ12 C1,2 C1,N ⎤ ⎥ ⎢ 2 C2,2 = σ 2 ⎥ ⎢ C2,1 ⎥ ⎢ O M ⎢ 2⎥ L CN ,N = σ N ⎦ ⎣ CN ,1
Summer School 2006 – Bologna, Italy. Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
23
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
24


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
25
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
26
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
27
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
28


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
29
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
30
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
31
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
32


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Summer School 2006 – Bologna, Italy.
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
33
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
34
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
What is a Kalman Filter?
• Optimal recursive data fusion algorithm • Predictor-Corrector style algorithm • Processes all available sensor measurements in estimating the value of parameters of interest using:
– Knowledge of system and sensor dynamics – Statistical models reflecting uncertainty in system noise and sensor dynamics – Any information regarding initial conditions
Summer School 2006 – Bologna, Italy.
What is a Kalman Filter (cont’d)?
• Optimal in the sense that for systems which can be described by a linear model, e.g.
v xk +1 = Axk + Buk + wk z k = Hxk + vk
and for which the process and measurement noises wk and vk are normally distributed, the Kalman filter is the provably optimal estimator (estimate has minimum error variance) • In our case, “process noise” corresponds to uncertainty in the motion model, measurement noise is from uncertainty in the sensing model, x denotes the state being estimated and z the sensor measurements
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
35
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
36


“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
What is a Kalman Filter (cont’d)?
• Recursive in the sense that it is “memory-less”
– Does not require all previous data to be maintained in memory and reprocessed at each time step – Propagates first and second order statistics only (i.e. mean and variance/covariance)
The Gaussian Distribution
• A 1-D Gaussian distribution is defined as:
p( x) = 1 2πσ 2
−
e
( x−µ )2 2σ 2
Gaussian p(x)
• In 2-D (assuming uncorrelated variables) this becomes:
r p( x ) = 1 2πσ 1σ 2
⎡ ( x − µ )2 ( x − µ )2 ⎤ − ⎢ 1 21 + 2 22 ⎥ 2σ 2 ⎥ ⎢ 2σ 1 ⎣ ⎦
• Crucial to the proof of optimality for the KF is that the process and measurement noise are normally distributed
e
p( w) ~ N (0, Q)
p(v) ~ N (0, R)
r p( x ) =
10/06/2006
• In n dimensions, it generalizes to: • With this assumption - and the linear process/measurement models – the uncertainty in the state estimate will also be normally distributed.
Summer School 2006 – Bologna, Italy.
1 ( 2π ) n | C |
e
1 − ( x − µ ) T C −1 ( x − µ ) 2
The Normal (Gaussian) distribution is completely parameterized by its first and second moments.
38
Summer School 2006 – Bologna, Italy.
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
37
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
“Kalman filtering (KF): theory and applications” applications”
Silvio Simani
Discrete Kalman Filter
• Estimate the state x ∈ ℜ of a linear stochastic difference equation x k = Axk −1 + Buk + wk −1
n
Estimates and Errors
n ˆ • x k ∈ ℜ is the estimated state at time-step k. n ˆ− • x k ∈ ℜ after prediction, before observation. ˆk e− = x k − x − • Errors: k ˆ ek = x k − x k • Error covariance matrices:
– process noise w is drawn from N(0,Q), with covariance matrix Q.
• with a measurement z ∈ ℜ m
z k = Hx k + vk
– measurement noise v is drawn from N(0,R), with covariance matrix R.
Pk− = E[e− e − ] k k
T
Pk = E[e k eT ] k
ˆ • Kalman Filter’s task is to update x k
Summer School 2006 – Bologna, Italy.
• A, Q are nxn. B is nxl. R is mxm. H is mxn.
Summer School 2006 – Bologna, Italy.
Pk
40
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”
39
10/06/2006
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION” NEUROREHABILITATION”


10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”4510/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”46
“Kalman filtering (KF): theory and applications”Silvio Simani“Kalman filtering (KF): theory and applications”Silvio Simani
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”5310/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”54
“Kalman filtering (KF): theory and applications”Silvio Simani“Kalman filtering (KF): theory and applications”Silvio Simani
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”5710/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”58
“Kalman filtering (KF): theory and applications”Silvio Simani“Kalman filtering (KF): theory and applications”Silvio Simani
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”6110/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”62
“Kalman filtering (KF): theory and applications”Silvio Simani“Kalman filtering (KF): theory and applications”Silvio Simani
http://www.ing.unife.it/simani/KF_lesson.html
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”65
“Kalman filtering (KF): theory and applications”Silvio Simani Kalman Filter MATLAB Function
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”6910/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”70
Kalman Filter Scheme
“Kalman filtering (KF): theory and applications”Silvio Simani“Kalman filtering (KF): theory and applications”Silvio Simani
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”7310/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”74
See Kalman Filtering for example that uses the kalman function.“Kalman filtering (KF): theory and applications”Silvio Simani 1-D Example: Estimating a Random Constant
10/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”7710/06/2006
Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO-MOTOR ASSESSMENT AND REHABILITATION”78
“Kalman filtering (KF): theory and applications”Silvio Simani“Kalman filtering (KF): theory and applications”Silvio Simani
10/06/2006Summer School 2006 –Bologna, Italy.
“ADVANCED TECHNOLOGIES FOR NEURO -MOTOR ASSESSMENT AND REHABILITATION ”81“Kalman filtering (KF): theory and applications ”Silvio Simani。