3D computer vision
Structure from motion
using Kalman filter
SFM Kalman V5c2
1
Aims
• To obtain structure and camera motion from
an image sequence
• Utilize inter-picture dynamics of a sequence
– Such as constant speed, acceleration of the
camera etc.
SFM Kalman V5c2
2
Tracking methods
• Kalman filtering, suitable for systems with
Gaussian noise
• Condensation (or called particle filter),
suitable for systems with Non-Gaussian
noise
SFM Kalman V5c2
3
Part 0
Basic concept of Kalman filter
SFM Kalman V5c2
4
Introduction
• A system (e.g. radar tracking a plane) can
be modelled by a system transition dynamic
function using the Newtons’ law (linear).
• Measurement may contain noise (assume
Gaussian)
• Kalman filter predict and update the system
to reduce the effect of noise.
SFM Kalman V5c2
5
System state and
dynamic
• xk is the state at time k
– xk=A*xk-1
– A is the motion model
– Q is system noise (by wind)
• Z is measurement on the
radar screen,
T
 du 
x  u,   [u, u ]T
 dt 
 [position, velocity] T
Newtons' law : uk  uk 1  u  t
So xk  Axk 1  Q , where
1 t 
A 
, hence
0
1


uk  1 t  uk 1 
xk     
Q





u
u
 k  0 1   k 1 
 Axk 1  Q
u 
du
 velocity
dt
u  position
– R is measurement noise
– Assume u>>h, so Zu
h
z
SFM Kalman V5c2
Radar
screen
6
Can add acceleration if we want
•
x  [u, u , u]T
 [position, velocity , accelerati on] T
1
2

Newtons' law : uk  uk 1  u  t  ut 
2
So xk  Axk 1  Q , where
1 t 0.5t 2 


A  0 1
t , hence
0 0
1 

uk  1 t 0.5t 

xk  uk   0 1
t
 
1
uk  0 0
2
 uk 1 
 
 uk 1   Axk 1  Q
 uk 1 

SFM Kalman V5c2
7
Kalman filter
• Always predict and update to find the state of the
plane x
• Kalman filter offers optimum prediction by
considering the system and measurement noise
ek+1=error
xˆk 1
xk+1
At time k+1
ek=error
x̂k
xk
At time k
SFM Kalman V5c2
ek-1=error
xˆk 1 ( predicted at time k-1)
xk-1(actual state at time k-1)
8
Part 1
Introduction to Kalman filter (KF) and
Extended Kalman filters (EKF)
SFM Kalman V5c2
9
Kalman filter introduction
•
•
Based on
An Introduction to the Kalman
FilterSourceTechnical Report: TR95-041 Year
of Publication: 1995 Authors Greg Welch Gary
Bishop Publisher University of North Carolina
at Chapel Hill Chapel Hill, NC, US
(http://www.cs.unc.edu/~welch/media/pdf/kalma
n_intro.pdf)
SFM Kalman V5c2
10
The process : state= position, velocity, acceleration
xk  Axk 1  Bu k  wk 1            (1)
xk (sizen1)  state at time is k
•
A(sizenn )  state_tran sition matrix
B(sizenn )  input gain
uk (sizen1)  input
w(sizen1)  state transitio n noise, prob(w)  N (0, Q )
**********************************
zk  Hx k  vk                  ( 2)
where, zk (sizem1)  measurements
H (sizemn )  state & measurement relation
v(sizem1)  measurement noise, prob(v)  N( 0 ,R)
SFM Kalman V5c2
11
Noise
w( size n1)  process noise
v ( sizem1)  measurement noise
E[ ] is the expected value (see wikipeida )
Qk , if i  k
E wk w  
 0, if i  k

T
i

Qk is called the covariance of process noise
Because wk , wi are independen t, so


E wk viT  0, for all k and i
Similarly
 Rk , if i  k
Evv 
 0, if i  k
Rk is called the covariance of measurement noise

T
k i

SFM Kalman V5c2
•
12
Kalman Computation: Using the current information (a priori) to
estimate the predicted future information (a posteriori)
• x is a state (e.g. position, velocity & acceleration, etc)
• z is measurement (image position [u,v]T etc)
a posterior state  xk | a priori state  xk
• k is time step index

k

k
a posterior est. state xˆk | a priori est. state  xˆk
e  xk  xˆ , where ˆ  estimate,  a priori
ek  xk  xˆk
-
a posterior err. cov. Pk | a priori err. cov. Pk
 
P  E e e   a posteriori estimate state error covariance
Pk  E ek ekT  a priori estimate state error covariance
k
T
k k
A posteriori state estimate is related to a priori estimate as
xˆk  xˆk  K k ( zk  Hxˆk )      (3)
Meaning of equ.(3) : Current est.state  previous est.state 
Kalman_gain (current measurement - H * previous est.state)
( zk  Hxˆk ) is called measurement innovation ( difference between zk & prediction )
from (2), zk  Hx k  vk , where xk is the current real state, put in (3)
xˆk  xˆk  K k ( Hx k  vk  Hxˆk )    ( 4)
K k ( sizenm )  Kalman gain or blending factor
SFM Kalman V5c2
13
How good is the prediction? We judge it by looking
at the error covariance: the smaller Pk the better.

Pk  E  xk  xˆk  xk  xˆk 
T
        (5)
a posterior state  xk | a priori state  xk
a posterior est. state xˆk | a priori est. state  xˆk
a posterior err. cov. Pk | a priori err. cov. Pk
Put (4) into (5)
Pk  E{[ xk  xˆk  K k ( Hxk  vk  Hxˆk )][ xk  xˆk  K k ( Hxk  vk  Hxˆk )]T }
Pk  E{[( xk  xˆk )  K k ( Hxk  Hxˆk )  K k vk ][( xk  xˆk )  K k ( Hxk  Hxˆk )  K k vk ]T }
Pk  E{[( I  K k H )( xk  xˆk )  K k vk ][( I  K k H )( xk  xˆk )  K k vk ]T }
Pk  E{( I  K k H )( xk  xˆk )( xk  xˆk )T ( I  K k H )T  E{ function() * ( xk  xˆk ) * vk )}
 K k E (vk vk T ) K kT
for K k and H k are constant, so E ( K k )  K k , E ( H )  H
Pk  ( I  K k H ) E{( xk  xˆk )( xk  xˆk )T }( I  K k H )T  E{ funtion() * ( xk  xˆk ) * vk )}
 K k E (vk vk T ) K kT
E{( xk  xˆk )( xk  xˆk )T }  Pk , E (vk vk T )  RK
( xk  xˆk ) and vk has no correlation, so E{ function() * (( xk  xˆk ) * vk )}  0
Pk  ( I  K k H ) Pk ( I  K k H )T  K k RK K kT            (6)
SFM Kalman V5c2
•
14
How to make Pk small?
Find optimal gain Kk to make Pk small
From(6), and tr{}  trace{}
From http://www.ee.ic.ac.uk/hp/staff/dmb/matrix/calculus.html
Pk  ( I  K k H ) Pk ( I  K k H )T  K k RK K kT    (6)
dyT  d ( yT )  (dy )T ; d (tr{ y})  tr{dy}
From http://en.wikipedia.org/wiki/Matrix_differentiation
Pk  ( Pk  K k HPk )( I  ( K k H )T )  K k RK K kT
Pk  Pk  Pk ( K k H )T  K k HPk  K k HPk ( K k H )T  K k RK K kT

k

k

k

k
Pk  P  P ( K k H )  K k HP  K k HP H K k  K k RK K
T
T
T
T
k
d (tr{ AB}) / dA  BT , AB must be sqaure
d (tr{ ACAT }) / dA  2 AC , C must be symmetric
Pk  Pk  K k HPk  Pk ( K k H )T  K k ( HPk H T  Rk ) K kT  (7)
Since trace of a matrix = trace of its transpose
tr{Pk }  tr{Pk }  2tr{K k HPk }  tr{K k ( HPk H T  Rk ) K kT }
so
d (tr{Pk })
 2( HPk )T  2 K k ( HPk H T  Rk )  0
dK k
result
K k  ( Pk H T )( HPk H T  R ) 1          (8)
This K k is called optimal gain.
Note: For Linear Kalman Filter, K(Kalman gain) and P (covariance) depend on
Q (system noise setting) and R(measurement noise setting) only and not Z
(measurement). So If Q ,R are given, K and P can be pre-calculated.
15
SFM Kalman V5c2
Error covariance Pk for update estimate
Put (8) back to (7)
K k  ( Pk H T )( HPk H T  R ) 1          (8)
Pk  Pk  K k HPk  Pk [ K k H ]T  K k ( HPk H T  Rk ) K kT  (7)
Pk  Pk  K k HPk  Pk H T K k T  K k ( HPk H T  Rk ) K kT
Pk  Pk  ( Pk H T )( HPk H T  R ) 1 HPk  Pk H T [( Pk H T )( HPk H T  R ) 1 ]T 
( Pk H T )( HPk H T  R ) 1 ( HPk H T  Rk )[( Pk H T )( HPk H T  R ) 1 ]T  (9)
The last term of (9)
( Pk H T )( HPk H T  R ) 1 ( HPk H T  Rk )[( Pk H T )( HPk H T  R ) 1 ]T
 ( Pk H T )[( Pk H T )( HPk H T  R) 1 ]T
So (9) becomes
Pk  Pk  ( Pk H T )( HPk H T  R ) 1 HPk  Pk H T [( Pk H T )( HPk H T  R ) 1 ]T 
( Pk H T )[( Pk H T )( HPk H T  R) 1 ]T
Pk  Pk  ( Pk H T )( HPk H T  R ) 1 HPk
Pk  ( I  K k H ) Pk          (10)
SFM Kalman V5c2
16
•
Error covariance prediction
ek  xk  xˆk , also ek1  xk 1  xˆk1 , by definition as described earlier
Because xk  Ak 1 xk 1  wk 1 ,
ek   Ak 1 xk 1  wk 1   Ak 1 xˆk1
ek  Ak 1ek 1  wk 1
The mean (expected value) of wk  0,

 
Since Pk  E ek ek   E  Ak 1ek 1  wk 1  Ak 1ek 1  wk 1 

 E  A
T
T

Pk  E  Ak 1ek 1  wk 1   Ak 1ek 1   wkT1
Pk
T


T
T
k 1ek 1  Ak 1ek 1   Ak 1ek 1wk 1  wk 1  Ak 1ek 1   wk 1  wk 1
T
T

Since wk 1 and ek 1 have no correlatio n, Ak 1ek 1wkT1  0, wk 1  Ak 1ek 1   0,
T

 
Pk  E  Ak 1ek 1  Ak 1ek 1   wk 1  wkT1  E  Ak 1ek 1 ekT1 AkT1   wk 1  wkT1
T





Pk  Ak 1  E ek 1ekT1  AkT1  E wk 1  wkT1  Ak 1Pk 1 AkT1  Q ,
or Pk  Ak 1Pk 1 AkT1  Q  (error covariance prediction )
SFM Kalman V5c2
Since A is constant , so
Pk  APk 1 AT  Q  (error covariance prediction )
•
17
Kalman filter (KF) recursive functions
a posterior state  xk | a priori state  xk
a posterior est. state xˆk | a priori est. state  xˆk
a posterior err. cov. Pk | a priori err. cov. Pk
• Prediction equations : project into next state
ˆxk  Axˆk 1  (state prediction )
Pk  APk 1 AT  Q  (error covariance prediction )
Update equations
Kalman gain K k  ( Pk H T )( HPk H T  R ) 1
Update Estimate xˆk  xˆk  K k ( zk  Hxˆk )
Update Covariance Pk  ( I  K k H ) Pk
SFM Kalman V5c2
18
Kalman Filter (KF) Iteration flow
Prediction equations : project into next state
xˆk  Axˆk 1  (state prediction )
Pk  APk 1 AT  Q  (error covariance prediction )
Update equations
Kalman gain K k  ( Pk H T )( HPk H T  R ) 1
Update Estimate xˆk  xˆk  K k ( zk  Hxˆk )
Initial states
xˆk 1and Pk 1
Update Covariance Pk  ( I  K k H ) Pk
SFM Kalman V5c2
19
A simple example of
Linear Kalman filter
•
•
•
•
•
Measure a noisy accelerometer output L
H=1 (scalar) measurement is the stated
x=Hz, hence x=z
Linear dynamic motion model A=1 (scalar)
Define the problem:
– By raw measurement z, we want to find L (the
true value of the accelerometer output)
– Q is system noise
– R is measurement noise
20
SFM Kalman V5c2
Kalman filter for an
accelerometer
•
SFM Kalman V5c2
21
Kalman Filter (KF) Iteration flow
All terms are scalar here, A=1, H=1,
and set Q=0.0001, R=0.1
Prediction equations : project into next state
xˆk  xˆk 1  (state prediction )
Pk  Pk 1  Q  (error covariance prediction )
Update equations
Kalman gain K k  ( Pk )( Pk  R ) 1
Initial states
xˆk 1and Pk 1
Update Estimate xˆk  xˆk  K k ( zk  xˆk )
Update Covariance Pk  ( I  K k ) Pk
SFM Kalman V5c2
22
Processing
set Q=0.0001, R=0.1,
Pk=0=1000 (random setting, but cannot be 0)
xk=0=0 (it is unknown, so set an arbitrary value)
• Predict:
Prediction equations : project into next state
xˆk  xˆk 1  (state prediction )
Pk  Pk 1  0.0001  (error covariance prediction )
• Update
Update equations
Kalman gain K k  ( Pk )( Pk  0.1) 1
Update Estimate xˆk  xˆk  K k ( zk  xˆk )
Update Covariance Pk  ( I  K k ) Pk
SFM Kalman V5c2
23
Time from K=1 to K=2
• Predict using Q=0.0001
– x_est_pos(k=2)= x_est_pos(k=1)= 0
(arbitury set)
• Set p_pos(1)=1000, Q=0.0001
– p_est_pri(k=2)=p_pos(k=1)+Q
– p_pri(2)=1000+0.0001=1000.0001
• Update using R=0.1,z=0.9
– K(2)=p_pri(2)*/(p_pri(2)+R)
• K(2)=1000.0001/(1000.0001+0.1)=0.9999
– x_est_pos(2)=x_est_pri(2)+K(2)*[zx_est_pri(2)]=
Prediction equations : project into next state
xˆk  xˆk 1  (state prediction )
Pk  Pk 1  Q  (error covariance prediction )
Q  0.0001 //set by user
Update equations
Kalman gain K k  ( Pk )( Pk  R ) 1
Update Estimate xˆk  xˆk  K k ( zk  xˆk )
Update Covariance Pk  ( I  K k ) Pk
R  0.1, //set by user
• x_est(2)=0+0.9999*[0.9-0]=0.8999
•
– p_pos(2)=(1-K)*p_pri(2)
• p_pos(2)=(1- 0.9999)*1000.0001=0.1
SFM Kalman V5c2
24
Kalman test
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
%Kalman test1
clear
'testi g kalman'
%generate data
kmax=100 % max time
x_real=1 * (ones(kmax,1)) %max x1 dimentation , assume a contant value
z=x_real+randn(kmax,1) %rand std=1,mean=0
clf
plot(z,'+')
hold on
plot(x_real)
%Init k=1
Q=0.0001
R=0.1
k=2
x_est_pos(1)= 0
p_pos(1)=1000;
% predict
x_est_pri(k)=x_est_pos(1);
p_pri(k)=p_pos(1)+Q;
% update;
Kgain(k)=p_pri(k)/(p_pri(2)+R);
x_est_pos(k)=x_est_pri(k)+ Kgain(k)*[z(k)-x_est_pri(k)];
p_pos(k)=(1-Kgain(k))*p_pri(k);
'test--'
k
x_est_pri
x_est_pos
p_pri
p_pos
Kgain
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
for k=2:kmax
x_est_pri(k)=x_est_pos(k-1);
% p_pos(k-1)=1000 ;
p_pri(k)=p_pos(k-1)+Q;
% update
Kgain(k)=p_pri(k)/(p_pri(k)+R)
x_est_pos(k)=x_est_pri(k)+Kgain(k)*[z(k)x_est_pri(k)]
p_pos(k)=(1-Kgain(k))*p_pri(k)
'test--'
k
x_est_pri
x_est_pos
p_pri
p_pos
Kgain
plot(x_est_pos,'o-')
% pause
end
SFM Kalman V5c2
25
Test result
•
Test1
–
–
•
–
–
Q = 0.00001, R= 0.1000
Measurment error is assumed to be
higher, trust the model more
x_est_pos fluctates less
circle line =x_est_pos
–
Solid line= True value
Test2
–
–
–
–
Q = 0.00001, R= 0.001000
Measurment error is assumed to be
lower, trust the measurmnet more:
x_est_pos fluctates more
circle line =x_est_pos
Solid line= True value
SFM Kalman V5c2
26
Time K=2
• Predict
•
Prediction equations : project into next state
xˆk  xˆk 1  (state prediction )
– x1=x0=0
– P1=1000+0.0001
Pk  Pk 1  Q  (error covariance prediction )
Q  0.0001 //set by user
• Update
– K1=1000.0001/(1000.000
1+0.1)=0.9999
– X1=0+0.9999(0.90)=0.8999
– P1=(10.9999)1000.0001=0.1
Update equations
Kalman gain K k  ( Pk )( Pk  R ) 1
Update Estimate xˆk  xˆk  K k ( zk  xˆk )
Update Covariance Pk  ( I  K k ) Pk
R  0.1, //set by user
SFM Kalman V5c2
27
Numerical results
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
%display
fprintf('Q=%5.5f;R=%5.5f\n',Q,R)
fprintf('x_real(%2d)=%5.5f;\n',1,x_real(1))
for k=1:kmax
fprintf('k=%2d;',k)
fprintf('x_est_pri(%2d)=%5.5f;',k,x_est_pri(k))
fprintf('x_est_pos(%2d)=%5.5f;',k,x_est_pos(k))
fprintf('p_pri(%2d)=%5.5f;',k,p_pri(k))
fprintf('p_pos(%2d)=%5.5f;',k,p_pos(k))
fprintf('Kgain(%2d)=%5.5f;',k,Kgain(k))
fprintf('z(%2d)=%5.5f;',k,z(k))
% fprintf('x_real(%2d)=%5.5f;',k,x_real(k))
fprintf('\n');
end
fprintf('\n');
%%the resulst is
Q=0.00010;R=0.00100
x_real( 1)=1.00000;
k= 1;x_est_pri( 1)=0.00000;x_est_pos( 1)=0.00000;p_pri( 1)=0.00000;p_pos( 1)=1000.00000;Kgain( 1)=0.00000;z( 1)=0.36584;
k= 2;x_est_pri( 2)=0.00000;x_est_pos( 2)=1.45851;p_pri( 2)=1000.00010;p_pos( 2)=0.00100;Kgain( 2)=1.00000;z( 2)=1.45851;
k= 3;x_est_pri( 3)=1.45851;x_est_pos( 3)=1.89194;p_pri( 3)=0.00110;p_pos( 3)=0.00052;Kgain( 3)=0.52381;z( 3)=2.28598;
k= 4;x_est_pri( 4)=1.89194;x_est_pos( 4)=1.51447;p_pri( 4)=0.00062;p_pos( 4)=0.00038;Kgain( 4)=0.38416;z( 4)=0.90935;
k= 5;x_est_pri( 5)=1.51447;x_est_pos( 5)=1.09337;p_pri( 5)=0.00048;p_pos( 5)=0.00033;Kgain( 5)=0.32622;z( 5)=0.22363;
k= 6;x_est_pri( 6)=1.09337;x_est_pos( 6)=0.41734;p_pri( 6)=0.00043;p_pos( 6)=0.00030;Kgain( 6)=0.29885;z( 6)=-1.16877;
k= 7;x_est_pri( 7)=0.41734;x_est_pos( 7)=0.95893;p_pri( 7)=0.00040;p_pos( 7)=0.00029;Kgain( 7)=0.28512;z( 7)=2.31684;
k= 8;x_est_pri( 8)=0.95893;x_est_pos( 8)=1.14236;p_pri( 8)=0.00039;p_pos( 8)=0.00028;Kgain( 8)=0.27804;z( 8)=1.61863;
k= 9;x_est_pri( 9)=1.14236;x_est_pos( 9)=1.22263;p_pri( 9)=0.00038;p_pos( 9)=0.00027;Kgain( 9)=0.27433;z( 9)=1.43496;
k=10;x_est_pri(10)=1.22263;x_est_pos(10)=1.63283;p_pri(10)=0.00037;p_pos(10)=0.00027;Kgain(10)=0.27237;z(10)=2.72864
;
SFM Kalman V5c2
28
•
Kalman code (lab5: IMU)
•
// Kalman filter module
•
•
•
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.03;
•
•
•
float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0,
P_11 = 0;
float dt, y, S;
float K_0, K_1;
•
•
SFM Kalman V5c2
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
float kalmanCalculate(float newAngle, float
newRate,int looptime) {
dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle *
dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;
y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle;}
29
Experiment using a real imu
title('Kalman IMU and sim: square line= arduino kalamn measured, +line =raw
measured, oline=matalb sim kalman')
•
SFM Kalman V5c2
30
Extended Kalman filter EKF
• For non-linear problems, such as 2D to 3D
computer vision problems.
• Here for our Structure from motion SfM
problem
– Camera motion is linear (Newtonian
mechanics)
– Measurement is non-linear (3D to 2D
projection)
SFM Kalman V5c2
31
Definitions:
Note: measurement to state is non-linear
 State Transition - x  structure state,  t  state noise,
vt  measuremen t noise
wˆ  state vector
xt  xt 1   t
 Measuremen t  t is the result of a non - linear function h(X)
 t  ht ( x )  vt , where vt  noise
x
Example ht ( x )  f  C
z
C
x
C
i
y
C
i
 is the i
C T
i
z
th
T
y 
, where
C 
z 
C
3 - D feature in camera coordinate s.
ht  non - linear measuremen t function , f  focal length
h
h 
 Jacobian of measuremen t
SFM Kalman V5c2
x
32
EKF Iteration flow
Ref: http://en.wikipedia.org/wiki/Extended_Kalman_filter
Prediction equations : project into next state for camera motion (linear)
xˆk  Axˆk1  (state prediction ), xˆk  f ( xˆk1 )  Axˆk1 , is a linear function
Pk  Fk 1  Pk 1  FkT1  Q , where F is the jacobian of f , f is linear so F  A
Pk  APk 1 AT  Q  (error covariance prediction )
EKF Update equations

Kalman gain K k  Pk h 
T
h P h 

k
T
Update Estimate xˆk  xˆk  K k zk  h xˆk ,0
R

1
Update Covariance Pk  I  K k h Pk
h is the jacobian of the measuremen t function h()
Initial states
xˆkand Pk
•
SFM Kalman V5c2
33
Part II
Two-pass Kalman filter for structure
and pose estimation
Description
Major reference [Yu, June 2005]
SFM Kalman V5c2
34
Two-pass Kalman filter for structure
and pose estimation
• Structure and pose updating
The i-th frame is not the last frame.
Increment i by 1.
Feature extraction
and tracking
using KLT
Model
Initialization
Set i=2
Step 1
Pose estimation
for the ith frame
Step 2
Structure updating
using N EKFs with
the ith image as the
measurement
The final
3D model
The last frame is reached
SFM Kalman V5c2
35
Model initializations
• f=focal length (found by camera calibration)
• Zinit=initial guess of z direction of the model points
(e.g. Zinit=1 meter from the camera)
ui
x
C
i
vi   pixel position measured
T
C
i
y
 is a 3 - D feature i in camera coordinate s.
C T
i
z
C
u

x
 i
f
i 
use the projection equation   
 C
v
z
 i
init  yi 
xiC , yiC , ( ziC  Zinit) can be guessed for all i : 1  m
x
C
i
C
i
y

C T
guess
i
z

 x
O
i
y
O
i

O T
object _ coordinates
i
z
SFM Kalman V5c2
36
Kalman tracking
Step 1: Kalman pose tracking
SFM Kalman V5c2
37
•
Kalman filtering pose tracking
Assume model is known (by
guessing)
X iC  ( RX iO  T )  T C , where
X iC  3  D model point at camera coordintes
X iO  3  D model point at object (world) coordintes
XiO = [xiO yiO ziO]’
Object coordinate
frame
v
Yo
Model
Zo
Oc
Xc
Zc
Tc
Oo
Camera coordinate
frame
Yc
C (image center)
u
Xo
SFM Kalman V5c2
38
•
Kalman pose filter
States
State transition
wˆ  state vector
w  state is defined as
Note: we use w instead of x for
state representation here


T
wˆ t ,t 1(121)  T1 , T1 , T2 , T2 , T3 , T3 , 1 , 1 , 2 , 2 , 3 , 3 , where

T1 , T2 , T3 are translati on along x, y, z axes resp. T1  dT1 dt etc;
1 , 2 , 3are rotation around x, y, z axes resp.
wˆ t (121)  A(1212) wˆ t 1(121)   t' (121) , where
wt  state vector,  t'  state transitio n noise
1 Ts 
1 Ts  
  diag 
, where Ts  dt
 ,  , 0 1  
0
1


 (1212)

SFM Kalman V5c2
39
Kalman pose filter
measurement
• measurement
f  focal length , vt'  measuremen t noise
 t'  gt ( wt )  vt'
image _ u 
image _ v   gt ( wt ) 


T
 x y 
x y 
 x y 
f   , ...  , ...  ,   (1)
z z 
 z z 
 z z 
C
1
C
1
C
1
C
1
C
i
C
i
SFM Kalman V5c2
C
i
C
i
C
n
C
n
C
n
C
n
40
Kalman for pose estimation
IMMKalmanPoseGen.m

SFM
Kalman
V5c2

T
wˆ t ,t 1(121)  state variables T1 , T1 , T2 , T2 , T3 , T3 , 1 , 1 , 2 , 2 , 3 , 3

T1 , T2 , T3 are translati on along x, y, z axes resp. T1  dT1 dt etc;
1 , 2 , 3are rotation around x, y, z axes resp.
N  number of features, K (122 N )  Kalman Gains
 1 Ts 1 Ts 1 Ts 1 Ts 1 Ts 1 Ts 
A  diag  
, 0 1 ,  0 1  , 0 1 , 0 1  ,  0 1     ( 2 )
0
1
 
 
 
 
 


for each image frame : loop teh follwoing for‧K_iter ations (min  1)
{wˆ t ,t 1(121) current  A(1212) wˆ t 1,t 1(121) previous                      (3)
Pt ,t 1(1212)  A(1212) Pt 1,t 1(1212) AT (1212)  Qt' (1212)                  ( 4)
- -Update equations - -

wˆ t ,t (121) next  wˆ t ,t 1(121) current  K (122 N )  t' ( 2 N )  gt ( wˆ t ,t 1 )( 2 N )

( 2 N 1)
         (5)
Pt ,t (1212)  Pt ,t 1(1212)  K (122 N )g w ( 2 N 12) Pt ,t 1(1212)                (6)

K (122 N )  Pt ,t 1(1212)g wT (122 N ) g w ( 2 N 12) Pt ,t 1(1212)g wT (122 N )  Rt' ( 2 N 2 N )
}

1
2 N 2 N
  ( 7)
41
•
Implementation
f  focal length , vt'  measuremen t noise
 t'  gt ( wt )  vt'
 image _ u1 
 image _ v 
1

T
  x1C y1C 
:


 xiC yiC 
 xnC ynC  

  gt ( wt )  f   C , C ...  C , C ...  C , C    (1)
:
 zi zi 
 zn zn  
  z1 z1 


image _ uN 


image
_
v
N


 u1 
v 
 1
 u2 
 
 v2 
: 
'
t   
 measuremen ts, e.g.obtain ed by KLT
:
 
: 
 
: 
u 
 N
 vN  2 N 1
SFM Kalman V5c2
42
Implementation details, equation 2,3
SFM Kalman V5c2
 1 Ts 1 Ts 1 Ts 1 Ts 1 Ts 1 Ts 
A1212  diag  
, 0 1 , 0 1 , 0 1 , 0 1 , 0 1  
0
1
 
 
 
 
 


Ts  dt


T
wˆ t ,t 1(121)  T1 , T1 , T2 , T2 , T3 , T3 , 1 , 1 , 2 , 2 , 3 , 3
wˆ t ,t 1(121) current  A(1212) wˆ t 1,t 1(121) previous                    (3)
T1 
1 Ts
 T1 
T 

 T 
1
1
 

 1 
T2 
1 Ts

 T2 


  
T
1
2
 

 T2 
T3 

 T3 
1 Ts


  
T
1
3
  
 T3 
   (3)
1 

 1 
1 Ts


  

1
 1

 1 
 

 2 
1 Ts
 2 

  

1
 2

 2 
 

1 Ts 3 
 3

 

1  3  t 1,t 1
3  t ,t 1 

T1 , T2 , T3 are translati on along x, y, z axes resp. T1  dT1 dt etc;
1 , 2 , 3are rotation around x, y, z axes resp.
43
State transition matrix A12x12
• Typical
• A12x12
• Ts=1 used in the
program
A1212
1 Ts



1


1 Ts




1




1 Ts


1




1 Ts


1




1 Ts


1



1 Ts


1 

SFM Kalman V5c2
44
Initialize pose_covar P1,1
• %Initialize state vector covariance for the pose filter
• pose_covar=P1,1 = diag([1 1 1 1 1 1 0.001 0.0001 0.1 0.01 0.01
0.001]*1000);
1000



1000


1000




1000




1000


1000


P1,1 


1


0
.
1




1


0
.
1




1


0.11212

SFM Kalman V5c2
45
•
Covariance of state noise= Q12x12 ,
Qp = diag([10 10 10 10 10 10 0.0001 0.0001 0.1 0.01 0.001 0.0001]);
Q1212
10



10


10




10




10


10




0.0001


0
.
0001




0 .1


0
.
01




0.001


0.00011212

SFM Kalman V5c2
46
•
•
Covariance of measurement noise=R12x12
Rc = diag(repmat([2 2], 1, n_features));
N=number of features
R2 N 2 N
2
 2

2


2













:
:
:
:













2

2

2 

2 2 N 2 N
SFM Kalman V5c2
47

Motion and projection

SFM Kalman V5c2
Ar time t  t1 , T  T1 T2 T3  is the translati on vector,
T
 r11 r12 r13 
R  r21 r22 r23  is the rotational matrix
r31 r32 r33 
The index t is dropped to simplify t he expression s.
After R,T  motion , model point [X,Y , Z ] is in the new 3 - D position [X',Y' , Z' ].
 X i '
 X i  T1   r11
 Y '   R  Y   T   r
 i 
 i   2   21
 Z i ' 
 Z i  T3   r31
The image projection qi 1,..., N
r12
r22
r32
r13   X i  T1 
r23   Yi   T2 
r33   Z i  T3 
are
captured by a camera of focal length f .
u    X ' 
qi   i    f  i ,
 vi    Z i ' 
 Y ' 
f  i 
 Z i ' 
After some manipulati ons
T
r11 X i  r12Yi  r13Z i  T1
X iR
X i  3Yi  2 Z i  T1
ui  f
 f R  f
r31 X i  r32Yi  r33Z i  T3
Zi
 2 X i  1Yi  Z i  T3
r21 X i  r22Yi  r23Z i  T2
Yi R
 X  Y  Z  T
vi  f
 f R  f 3 i i 1 i 2
r31 X i  r32Yi  r33Z i  T3
Zi
 2 X i  1Yi  Z i  T3
48
•
Pose Jacobian Matrix g w( 2 N 12)
Given ui  gu (1 , 2 , 3 , T1 , T2 , T3 , X i , Yi , Z i ) 
SFM Kalman V5c2
r11 X i  r12Yi  r13Z i  T1
X iR
X i  3Yi  2 Z i  T1
ui  f
 f R  f
r31 X i  r32Yi  r33Z i  T3
Zi
 2 X i  1Yi  Z i  T3
r21 X i  r22Yi  r23Z i  T2
Yi R
 X  Y   Z  T2
vi  f
 f R  f 3 i i 1 i
r31 X i  r32Yi  r33Z i  T3
Zi
 2 X i  1Yi  Z i  T3
 r11 r12 r13   1  3 2 
since R  r21 r22 r23    3
1  1 

 

1 
 r31 r32 r33    2 1
 ui 1 ui 1 ui 1 ui 1 ui 1
 T
T1
T2
T2
T3
1

 vi 1 vi 1 vi 1 vi 1 vi 1
 T1
T1
T2
T2
T3
 u
ui 2 ui 2
:
g w ( 2 N 12)   i 2


T

T

T
1
1
2

 vi 2 vi 2 vi 2
:
 T


T

T
1
2
 :1
:

 :
:
ui 1
T3
vi 1
T3
ui 1
1
vi 1
1
ui 1
1
vi 1
1
ui 1
2
vi 1
2
ui 1
2
vi 1
2
ui 1
3
vi 1
3
ui 1 
3 

vi 1 
3 

2 N  12






Each feature generates 2 rows, total 2 N rows. Each row has 12 columns.
u
u
u
u
u
u
In practice all


      0
T1 T2 T3 1 2 3
and
v
v
v
v
v
v


      0
T1 T2 T3 1 2 3
49
•
Yu-recursive Kalman [Yu, June 2005] algorithm
Matlab for pose Jacobian matrices (full)
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
%oct 2014
%************************************************************************
function TestJacobian_pose_full
% Try to solve the differentiate equations without simplification
clc,clear;
disp('TestJacobian');
•
%************************
syms a b c; % a= pitch ( around x axis), b=yaw(around y), c=roll (around z) respectively
syms f X Y Z T1 T2 T3;
F=[
%u
f*((cos(b)*cos(c)*X - cos(b)*sin(c)*Y + sin(b)*Z+ T1)...
/((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X ...
+ (cos(a)*sin(b)*sin(c)+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3));
%v
((sin(a)*sin(b)*cos(c)+ cos(a)*sin(c))*X ...
+ (-sin(a)*sin(b)*sin(c)+ cos(a)*cos(c))*Y - sin(a)*sin(b)*Z + T2)...
/((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X + (cos(a)*sin(b)*sin(c)...
+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3)]
V = [a,b,c,T1,T2,T3];
Fjaco = jacobian(F,V);
disp('Fjaco =');
disp(Fjaco);
size(Fjaco) % Fjaco is 2x6
%Fjaco(1,1)
SFM Kalman V5c2
50
SFM
Kalman
V5c2
Yu-recursive Kalman [Yu, June 2005] algorithm
Matlab for pose Jacobian matrices (approximation)
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
% TestJacobian_pose_approx
%function TestJacobian_pose_approx
%'==test jacobian for chang,wong ieee_mm 2 pass lowe=================='
%use twist (small) angles approximation.
clear, clc;
syms R dR M TT XYZ ZZ x y z f u v a1 a2 a3 t1 t2 t3 aa1 aa2 aa3 tt1 tt2 tt3
R=[1 -aa3 aa2; aa3 1 -aa1; -aa2 aa1 1];
dR=[1 -a3 a2; a3 1 -a1; -a2 a1 1];
M=[x;y;z];
TT=[tt1;tt2;tt3];
dt=[t1;t2;t3]
XYZ=dR*R*M+TT+dt; % R is a matrix multiplication transform
u=f*XYZ(1)/XYZ(3);
v=f*XYZ(2)/XYZ(3);
ja=jacobian([u ;v],[tt1 t1 tt2 t2 tt3 t3 aa1 a1 aa2 a2 aa3 a3])
size(ja) %size of ja is 2x12
ja(1,1),ja(1,2),ja(1,3),ja(1,4),ja(1,5),ja(1,6),ja(1,7),ja(1,8),ja(1,9),ja(1,10),ja(1,11),ja(1,12)
51
Kalman tracking
Step2: Kalman structure tracking
SFM Kalman V5c2
52
Kalman filtering structure tracking
Assume pose is known
•
X iC  ( RX iO  T )  T C , where
X iC  3  D model point at camera coordintes
X  3  D model point at object (world) coordintes
O
i
XiO = [xiO yiO ziO]’
Object coordinate
frame
v
Yo
Oc
Xc
Zc
Tc
Model
Oo
Camera coordinate
frame
Yc
C (image center)
u
Xo
Zo
SFM Kalman V5c2
53
Kalman structure filter
States
• Structure State transition
wˆ  state vector
For feature_in dex  1 to N
{  State Transition - X  structure state,  t  state noise,
vt  measuremen t noise
X t  X t 1   t
  Measuremen t  
 t  ht ( X )  vt
 xC y C 
ht ( X )  f  C
z C 
z
ht  measuremen t function , f  focal length
}
SFM Kalman V5c2
54
Extended Kalman filter iteration for the model structure
The process is an identify matrix (no change of structure
against time)
The procedure is for each features:
For feature_in dex  1 to N{
Prediction equations for structure states
Xˆ
 Xˆ
t ,t 1(11)
t 1,t 1(11)
 t ,t 1( 31)   t 1,t 1  Qt ,
where Qt  noise covariance ,   error covariance
W  Kalman gain, hX  Jacobian for model
- -Update equations - Xˆ
 Xˆ
W
t ,t (11)
t 1,t 1(11)
(12 )

'
t

 ht ( Xˆ t ,t 1 ) ( 21)
 t ,t (11)   t 1,t 1(11)  W(12 ) (hX )( 21)  t 1,t 1(11)

W(12 )   t ,t 1(11) (hXT )(12 ) hX ( 21)  t ,t 1(11) (hXT )(12 )  Rt ( 22 )

1
}
SFM Kalman V5c2
55
Yu-recursive Kalman [Yu, June 2005] algorithm
Matlab for structure Jacobian matrices (full)
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
•
% Solve the problem without simplification
% Since is the yaw, pitch, roll of the rotation respectively. The R can be represent by as:
%
% Substitute R into (1), we have the following program (in matlab):
%************************************************************************
function TestJacobian
% Try to solve the differentiate equations without simplification
clc,clear;
disp('TestJacobian');
syms a b c; %yaw( around x axis), pitch(around y), roll(aroudn z) respectively
syms f X Y Z T1 T2 T3;
F=[
%u
f*((cos(b)*cos(c)*X - cos(b)*sin(c)*Y + sin(b)*Z+ T1)...
/...
((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X ...
+ (cos(a)*sin(b)*sin(c)+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3));
%v
((sin(a)*sin(b)*cos(c)+ cos(a)*sin(c))*X ...
+ (-sin(a)*sin(b)*sin(c)+ cos(a)*cos(c))*Y - sin(a)*sin(b)*Z + T2)...
/...
((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X + (cos(a)*sin(b)*sin(c)...
+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3)]
V = [X,Y,Z];
Fjaco = jacobian(F,V);
%************************
SFM Kalman V5c2
56
Structure Jacobian
•
function lpf = KalmanStructJacobSP(RT_solution, a, f, u, v, initz)
•
•
•
•
•
•
•
b = 1/f;
tx = RT_solution(1);
ty = RT_solution(2);
tzxb = RT_solution(3);
wx = RT_solution(4);
wy = RT_solution(5);
wz = RT_solution(6);
•
temp1 = 1 - b*sin(wy)*(u + a*b*u) + b*cos(wy)*sin(wx)*(v + a*b*v) + b*cos(wy)*cos(wx)*(a initz) + tzxb;
•
•
•
•
•
•
•
•
•
lpf = zeros(2, 1);
lpf(1) = (cos(wz)*cos(wy)*b*u + (cos(wz)*sin(wy)*sin(wx) - sin(wz)*cos(wx))*b*v +
cos(wz)*sin(wy)*cos(wx) + sin(wz)*sin(wx))/(temp1) - ...
(cos(wz)*cos(wy)*(u + a*b*u) + (cos(wz)*sin(wy)*sin(wx) - sin(wz)*cos(wx))*(v + a*b*v)
+ ...
(cos(wz)*sin(wy)*cos(wx) + sin(wz)*sin(wx))*(a - initz) + tx)*(((-b)^2)*sin(wy)*u +
(b^2)*cos(wy)*sin(wx)*v + b*cos(wy)*cos(wx)) /(temp1^2);
lpf(2) = (sin(wz)*cos(wy)*b*u + (sin(wz)*sin(wy)*sin(wx) + cos(wz)*cos(wx))*b*v +
sin(wz)*sin(wy)*cos(wx) - cos(wz)*sin(wx))/(temp1) - ...
(sin(wz)*cos(wy)*(u + a*b*u) + (sin(wz)*sin(wy)*sin(wx) + cos(wz)*cos(wx))*(v + a*b*v)
+ ...
(sin(wz)*sin(wy)*cos(wx) - cos(wz)*sin(wx))*(a - initz) + ty)*(((-b)^2)*sin(wy)*u +
(b^2)*cos(wy)*sin(wx)*v + b*cos(wy)*cos(wx)) /(temp1^2);
SFM Kalman V5c2
57
return
Yu-recursive Kalman [Yu, June 2005] algorithm
Matlab for structure Jacobian matrices (approximate))
•
•
•
•
•
'====test jacobian for structure================='
%use twist (small) angles approximation.
clear, clc;
syms R dR M TT XYZ ZZ x y z f u v a1 a2 a3 t1 t2 t3 aa1 aa2 aa3
tt1 tt2 tt3
R=[1 -aa3 aa2; aa3 1 -aa1; -aa2 aa1 1];
dR=[1 -a3 a2; a3 1 -a1; -a2 a1 1];
M=[x;y;z];
TT=[tt1;tt2;tt3];
dt=[t1;t2;t3]
XYZ=dR*R*M+TT+dt; % R is a matrix multiplication transform
u=f*XYZ(1)/XYZ(3);
v=f*XYZ(2)/XYZ(3);
•
•
•
•
•
•
•
•
•
• ja=jacobian([u ;v],[x y z])
•
•
SFM Kalman V5c2
58
Part B
Interacting Multiple Model Method
(IMM) structure and pose estimation
Major reference [Yu
SFM Kalman V5c2
Aug.2005]
59
Example: Interacting Multiple Model
Method (IMM) for (IMM-KSFM)
•
3 dynamic models =3 parallel Kalman structures
–
•
•
•
General, Rotation, Translation
Use IMM to find the suitable dynamic model at time t
Each Kalman structure is similar to IV-KSFR
Reference
1) Ying Kin Yu, Kin Hong Wong and Michael
Ming Yuen Chang, "Merging Artificial
Objects with Marker-less Video Sequences
Based on the Interacting Multiple Model
Method", IEEE Transactions on Multimedia,
Volume 8, Issue 3, June 2006 Page(s):521 528.
SFM Kalman V5c2
60
Steps
SFM Kalman V5c2
61
Pose
SFM Kalman V5c2
62
IMM switching
SFM Kalman V5c2
63
Part C
USE Trifocal tensor (TRI-KSFM)
structure and pose estimation
Major reference [Yu
SFM Kalman V5c2
Feb.2005]
64
Example : USE Trifocal tensor (TRIKSFM)
•
•
•
•
•
Always use first second (I1, I2) frames as reference
Use It as the third frame to form a trifocal tensor at time t
If only pose output required, use one Kalman filter
If structure is needed use N Kalman filters for N features,
similar to IV-KSFM.
Reference
1) Ying Kin YU, Kin Hong Wong and Michael
Ming Yuen Chang, "Recursive 3D Model
Reconstruction Based on Kalman Filtering",
IEEE Transactions on Systems, Man and
Cybernetics B, Vol.35, No.3, June 2005.
SFM Kalman V5c2
65
TRI-KSFM flow diagram
SFM Kalman V5c2
66
The flow of the recursive algorithm
with the structure recovery extension.
SFM Kalman V5c2
67
Condensation
1) Reference
1)
Kin-Hong Wong and Michael Ming-Yuen Chang, "Pose tracking for
virtual walk-through environment construction", Proceedings of the
Recent Development in Theories and Numerics, Editor: Y.C. Hon ,
World Scientific Publishing Co. Pte. Ltd. 2003. ISBN 981-238-266-2
SFM Kalman V5c2
68
Conclusion
• Studied various techniques for model and
pose tracking in 3D computer vision.
SFM Kalman V5c2
69
References on Kalman filter
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
[Azarbayejani 1995] A.Azarbayejani and A.P.Pentland, “Recursive estimation of motion, structure, and focal length”, IEEE Trans. on
PAMI, vol. 17, no 6, June 1995.
[Azarbayejani 1993] A. Azarbayejani, T. Starner, B. Horowitz, A. Pentland,“, Visually Controlled Graphics“, IEEE Transactions on
Pattern Analysis and Machine Intelligence, vol.15, no. 6, pages 602-605,1993.
[Yu, June 2005] Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "Recursive 3D Model Reconstruction Based on
Kalman Filtering", IEEE Transactions on Systems, Man and Cybernetics B, Vol.35, No.3, June 2005.
Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "A Fast and Robust Simultaneous Pose Tracking and Structure
Recovery Algorithm for Augmented Reality Applications", International Conference on Image Processing (ICPR04), Singapore, Oct.
2004.
Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang,"A Fast Recursive 3D Model Reconstruction Algorithm for Multimedia
Applications", International Conference on Pattern Recognition ICPR2004, Cambridge, August 2004.
Ying Kin Yu, Kin Hong Wong and Michael Ming Yuen Chang, "Merging Artificial Objects with Marker-less Video Sequences Based
on the Interacting Multiple Model Method", IEEE Transactions on Multimedia, Volume 8, Issue 3, June 2006 Page(s):521 - 528.
Ying Kin YU, Kin Hong Wong and Michael Ming Yuen Chang, "Recursive 3D Model Reconstruction Based on Kalman Filtering",
IEEE Transactions on Systems, Man and Cybernetics B, Vol.35, No.3, June 2005.
Kin-Hong Wong and Michael Ming-Yuen Chang, "Pose tracking for virtual walk-through environment construction", Proceedings of the
Recent Development in Theories and Numerics, Editor: Y.C. Hon , World Scientific Publishing Co. Pte. Ltd. 2003. ISBN 981
Greg Welch and Gary Bishop, "An Introduction to the Kalman Filter, "TR 95-041 Department of Computer Science University of North
Carolina at Chapel Hill Chapel Hill, NC 27599-3175 www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf (excellent tutorial)
T.J.Broida, S.Chandrasiiekhar and R.Chellappa, “Recursive 3-D motion estimation from monocular image sequence”, IEEE Trans. on
Aerospace and Electronic Systems, vol 26, no. 4, July 1990. (The first paper of Kalman on pose estimation)
http://mathworld.wolfram.com/Quaternion.html
http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm
A.W. Rihaczek, “Recursive methods for the estimation of rotation quaternions”, IEEE transactions on aerospace and electronic systems,
Vol. 32, No. 2, April 1996.
[Yu, Feb 2005]Ying Kin Yu, Kin Hong Wong, Michael Ming Yuen Chang and Siu Hang Or, "Recursive Camera Motion Estimation
with Trifocal Tensor:, Submitted to a journal.
SFM Kalman V5c2
70
Appendix
Some math background
•
•
•
•
Mean
Variance/ standard deviation
Covariance
Covariance matrix
SFM Kalman V5c2
71
Mean, variance (var) and
standard_deviation (std)
1 n
mean    x    xi
n i 1
2
n
1
var( x) 
   xi   
(n  1) i 1
std ( x)  var( x)
•
•
•
•
•
•
•
•
•
•
•
•
•
•
x=
2.5000
0.5000
2.2000
1.9000
3.1000
2.3000
2.0000
1.0000
1.5000
1.1000
mean_x = 1.8100
var_x = 0.6166
std_x = 0.7852
%matlab code
x=[2.5 0.5 2.2 1.9 3.1 2.3 2 1 1.5 1.1]'
mean_x=mean(x)
var_x=var(x)
std_x=std(x)
x
%%%%%%%%Answer:
mean_x = 1.8100
var_x = 0.6166
std_x = 0.7852
SFM Kalman V5c2
sample
72
N or N-1 as denominator??
see
http://stackoverflow.com/questions/3256798/why-does-matlab-native-functioncov-covariance-matrix-computation-use-a-differe
• “n-1 is the correct denominator to use in
computation of variance. It is what's known as
Bessel's correction”
(http://en.wikipedia.org/wiki/Bessel%27s_corr
ection) Simply put, 1/(n-1) produces a more
accurate expected estimate of the variance
than 1/n
SFM Kalman V5c2
73
1 n
mean    x    xi
n i 1
Class exercise 1
By computer (Matlab)
•
•
•
•
•
•
•
•
x=[1 3 5 10 12]'
mean(x)
var(x)
std(x)
Mean(x)
= 6.2000
Variance(x)= 21.7000
Stand deviation = 4.6583
2
n
1
var( x) 
   xi   
(n  1) i 1
std ( x)  var( x)
By and
• x=[1 3 5 10 12]'
• mean=
• Variance=
• Standard deviation=
SFM Kalman V5c2
%class exercise1
x=[1 3 5 10 12]'
mean(x)
var(x)
std(x)
74
1 n
mean    x    xi
n i 1
Answer1:
2
n
1
var( x) 
   xi   
(n  1) i 1
std ( x)  var( x)
By computer (Matlab)
•
•
•
•
•
•
•
•
x=[1 3 5 10 12]'
mean(x)
var(x)
std(x)
Mean(x)
= 6.2000
Variance(x)= 21.7000
Stand deviation = 4.6583
By hand
• x=[1 3 5 10 12]'
• mean=(1+3+5+10+12)/5
• =6.2
• Variance=((1-6.2)^2+(36.2)^2+(5-6.2)^2+(106.2)^2+(12-6.2)^2)/(51)=21.7
• Standard deviation=
sqrt(21.7)= 4.6583
SFM Kalman V5c2
75
What do mean, variance,
standard deviation tell you
• Mean: the average value of a set of numbers
• Variance: the sum of the square of the
difference between the numbers and the
mean.
• Stand deviation : the square root of the
mean , a measurement of how the numbers
are deviated from the mean. Give you
intuition than the Variance.
SFM Kalman V5c2
76
Variance/Covariance matrix (or called Covariance
matrix) of a vector of random variables
•
•
See
http://www.biostat.jhsph.edu/~fdominic/teaching/bio655/extras/matrixnotes2.pdf
Y is a vector of random variables with zero mean
Y  [ y1 , y2 , y3 , y4 ]'
 i2  variance of yi
cov ij  variance between Yi and Y j , i  j
covariance _matrixof X is 
  i2

cov
   12
 :

cov1n
.. cov1n 

 i2 .. cov 2 n 
:
.
: 

cov 2 n ..  n2 
 is symmteric matrix and the digonal are the
cov12
variances
SFM Kalman V5c2
77
Variance -covariance matrix
• Videos
• Co-variance matrix and correlation
– https://www.youtube.com/watch?v=ZfJW3ol2F
bA
• Co-variance matrix
– https://www.youtube.com/watch?v=locZabK4
Als
SFM Kalman V5c2
78
Particle filter
• Video
– https://www.youtube.com/watch?v=OlAJVra1PU
– With matlab code:
• https://www.youtube.com/watch?v=HZvF8KlFoWk
SFM Kalman V5c2
79