Control Theory
Extended Kalman Filter
2015 Spring
Motion & Power Control Laboratory
Nonlinear System Model
• Nonlinear system model
xk 1  f ( xk )  wk
zk  h( xk )  vk
• Difference from the linear model
A xk  f ( xk )
H xk  h( xk )
Extended Kalman Filter(EKF) Algorithm
0. Determine initial values
xo , Po
Recursive k-th iteration
I. Predict the Estimate Value and the Covariance
xk  f ( xk 1 )
Pk  A Pk 1 AT  Q
H
h
 x x

k
3
II. Calculate Kalman Gain

k


k
Kk  P H H P H  R
Measured
Value
zk
T
T

1
Estimate
Value
III. Calculate the Estimate Value

k


k
xk  x  Kk zk  h ( x )

A
f
x
xk
x k
IV. Calculate the Covariance
Pk  Pk  Kk H Pk
Go to Step I
4
Linearized Jacobian Matrix
A
H
f
x
x k
h
 x x

k
Variables of Kalman Filter
Input
zk
Output
xk
System Model
f , h, Q, R
Internal Calculation



A, H , xk , Pk , Pk , Kk
6
Radar Tracking
• State variables
horizontal distance  x1 

  
x   moving speed    x2 

 x 
altitude

  3
• Suppose that the moving speed and the altitude are
constant, then the system model is given by
 x1  0 1 0 x1   0 
  
   

 x2   0 0 0 x2    w1   A x  w
 x  0 0 0 x  w 
 3   2 
 3 
7
• Radar measurement
r  x12  x32    h( x)  
• Discrete system model
 x1 
 x1  0 1 0  0 
 
  
 t   w 
x

x

0
0
0
 2
 2 
  1
x 
 x  0 0 0 w 
  2 k
 3 k 1  3 k 
• Jacobian
  h  h  h   x1
H 
 2 2 0
  x1  x2  x3   x1  x3


2
2
x1  x3 
x3
8
Kalman Filter Function: RadarEKF.m
function [pos vel alt] = RadarEKF(z,dt)
persistent A Q R x P firstRun
if isempty(firstRun)
A=eye(3)+dt*[0 1 0; 0 0 0; 0 0 0];
Q=[0 0 0; 0 0.001 0; 0 0 0.001];
R=10;
x=[0 90 1100]’;
P=10*eye(3);
firstRun=1;
End
9
H=Hjacob(x);
xp=A*x;
% I.
Predict the estimate
Pp=A*P*A’+Q;
%
Predict the error covariance
K=Pp*H’*inv(H*Pp*H’+R);
% II. Calculate the Kalman gain
x=xp+K*(z-hx(xp));
% III. Calculate the estimate
P=Pp-K*H*Pp;
% IV. Calculate the error covariance
pos=x(1);
vel=x(2);
Alt=x(3);
10
function zp = hx(xhat)
x1=xhat(1);
x3=xhat(3);
zp= sqrt(x1^2+x3^2);
function H = Hjcob(xp)
H=zeros(1,2);
x1=xp(1);
x3=xp(3);
H(1)= x1/sqrt(x1^2+x3^2);
H(2)=0;
H(3)= x3/sqrt(x1^2+x3^2);
11
Test: Radar Tracking
TestRadarEKF.m
clear all
dt=0.05;
t=0:dt:20;
Nsamlpes=length(t);
Xsaved=zeros(Nsamples,1);
Zsaved=zeros(Nsamples,1);
PosSaved=Xsaved(:,1);
VelSaved=Xsaved(:,2);
AltSaved=Xsaved(:,3);
t=0:dt:Nsamples*dt-dt;
filgure
plot(t,PosSaved,t,VelSaved,t,AltSaved)
For k=1:Nsamples
r=GetRadarEKF(dt);
[pos vel alt]=RadarEKF(r,dt);
Xsaved(k,:)=[pos vel alt];
Zsaved(k)=r;
Algorithm
Test
program
RadarEKF.m
Main file
TestRadarEKF.m
sensor
GetRadarEKF.m
end
12
GetRadar.m
function r = GetRadar(dt)
Persistent posp
If isempty(posp)
posp=0;
End
vel=100+5*randn;
alt=1000+10*randn;
pos=pos+vel*dt;
v=0+pos*0.05*randn;
r=sqrt(pos^2+alt^2)+v;
posp=pos;
13
110
105
Speed ( m/sec )
100
95
90
85
80
0
2
4
6
8
10
Time ( sec )
12
14
16
18
20
14
1060
1050
Altitude ( m )
1040
1030
1020
1010
1000
0
2
4
6
8
10
Time ( sec )
12
14
16
18
20
15
2500
Horizontal position ( m )
2000
1500
1000
500
0
0
2
4
6
8
10
Time ( sec )
12
14
16
18
20
16
2600
Measured
Estimated
2400
2200
Radar disctance ( m )
2000
1800
1600
1400
1200
1000
800
0
2
4
6
8
10
Time ( sec )
12
14
16
18
20
17
Attitude Determination of a Chopper
• System model
  1 sin tan cos tan   p
  
 q   w


0
cos

sin

  
 
  0 sin / cos cos / cos   r 
 
 p  q sin  tan   r cos tan  
  w  f ( x)  w

q cos  r sin 


 q sin  sec   r cos sec  
 
1 0 0  
z
     H x  

0 1 0  
 
18
Jacobian matrix
f
A
x
x k
  f1

 
 f2


 
 f
 3
  
 f1

 f2

 f3

 f1 
  
 f2 
 
 f3 

   x
k
2
2
q cos  tan   r sin  tan 
q sin  sec    r cos  sec  
0



 q sin   r cos 
0
0
 q cos  sec   r sin  sec  q sin  sec  tan   r cos  sec  tan  0

 xk
Kalman Filter Function: EulerEKF.m
function [phi theta psi] = EulerEKF(z,rates,dt)
persistent H Q R x P firstRun
if isempty(firstRun)
firstRun=1;
H=[1 0 0; 0 1 0];
Q=0.0001*eye(3);
x=xp+K*(z-H*xp);
P=Pp-K*H*Pp;
phi
= x(1);
theta = x(2);
psi
= x(3);
R=6*eye(2);
x=[0 0 0]’;
P=10*eye(4)
End
A=Ajacob(x, rates, dt);
xp=fx(x,rates,dt);
Pp=A*P*A’+Q;
K=Pp*H’*inv(H*Pp*H’+R);
20
function xp = fx(xhat, rates, dt)
function A = Ajacob(xhat, rates, dt)
phi = xhat(1);
A=zeros(3,3);
theta=xhat(2);
phi=xhat(1);
p=rates(1);
theta=xhat(2);
q=rates(2);
p=rates(1);
r=rates(3);
q=rates(2);
xdot=zeros(3,1);
r=rates(3);
xdot(1)=p+q*sin(phi)*tan(theta)
A(1,1)=q*cos(phi)*tan(theta)-r*sin(phi)*tan(theta);
+r*cos(phi)*tan(theta);
A(1,2)=q*sin(phi)*sec(theta)^2+r*cos(phi)*sec(theta)^2;
xdot(2)= q*cos(phi) -r*sin(phi);
A(1,3)=0;
xdot(3)= q*sin(phi)*sec(theta)
A(2,1)=-q*sin(phi)-r*cos(phi);
+r*cos(phi)*sec(theta);
xp=xhat+xdot*dt;
A(2,2)=0;
A(2,3)=0;
A(3,1)=q*cos(phi)*sec(theta)-r*sin(phi)*sec(theta);
A(3,2)=q*sin(phi)*sec(theta)*tan(theta)
+r*cos(phi)*sec(theta)*tan(theta);
A(3,3)=0;
A=eys(3)+A*dt;
21
Test: Attitude Determination Using Sensor Fusion
TestEulereEKF.m
phiSaved=EulerSaved(:,1)*180/pi;
Nsamples=41500;
thetaSaved=EulerSaved(:,2)*180/pi;
psiSaved-EulerSaved(:,3)*180/pi
EulerSaved=zeros(Nsamples,3)
t=0:dt:Nsamples*dt-dt;
dt=0.01;
figure
For k=1:Nsamples
plot(t, PhiSaved)
[p q r]=GetGyro();
figure
[ax,ay]=GetAccel();
plot(t, ThetaSaved)
[phi_a theta_a]=EulerAccel(ax,ay);
figure
[phi theta psi]=EulerEKF([phi_a theata_a]’,
plot(t,PsiSaved)
[p q, r], dt);
EulerSaved(k,:)=[phi theta psi];
End
Algorithm
Test
program
EulerEKF.m
Main file
TestEulerEKF.m
sensors
GetGyro.m,
GetAccel.com
22
40
30
Roll angle ( degree )
20
10
0
-10
-20
-30
0
50
100
150
200
250
Time ( sec )
300
350
400
450
0
50
100
150
200
250
Time ( sec )
300
350
400
450
30
Pitch angle ( degree )
20
10
0
-10
-20
-30
23