engr 9826 ass2 report
Post on 06-Apr-2018
223 Views
Preview:
TRANSCRIPT
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 1/28
ENGR 9826
Advanced Control System
Assignment-2, March 08, 2012
Thumeera Ruwansiri, Wanasinghe Arachchige
Student # 201095700
Faculty of Engineering and Applied Science
Memorial University of Newfoundland
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 2/28
Contents
1 Answer for Question 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Answer for Question 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.1 Comparison of implementation of KF (Algorithm 1) and UKF (Algorithm 2) . . . . . 26
2
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 3/28
List of Figures
1 Comparison of actual, estimated and filtered value for X(1) . . . . . . . . . . . . . . . . . . . 9
2 Comparison of actual, estimated and filtered value for X(2) . . . . . . . . . . . . . . . . . . . 10
3 Comparison of actual, estimated and filtered value when Qn=2000 and Rn=0.1 . . . . . . . . 11
4 Comparison of actual, estimated and filtered value when Qn=0.0002 and Rn=0.1 . . . . . . . 11
5 Comparison of actual, estimated and filtered value when Qn=0.2 and Rn=1000 . . . . . . . . 12
6 Comparison of actual, estimated and filtered value when Qn=0.2 and Rn=0.0001 . . . . . . . 12
7 Comparison of actual, estimated and filtered value when Qn=0.1 and Rn=0.1 . . . . . . . . . 13
8 Comparison of actual, estimated and filtered value when A=[-0.8 1 ; 1.2 -2.3] . . . . . . . . . 14
9 Comparison of actual, estimated and filtered value when A=[-1.5 1 ; -1.2 -0.3] . . . . . . . . . 15
10 Comparison of actual, estimated and filtered value when A=[-0.1 -1 ; 1.2 0] . . . . . . . . . . 16
11 Comparison of actual, estimated and filtered value when a plat (state variables) is(are) not
fully observable . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
12 Four tank system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
13 Comparison of actual and filtered value for four tank system . . . . . . . . . . . . . . . . . . . 25
3
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 4/28
List of Tables
1 Different scenarios where the process and noise covariance term are in noise . . . . . . . . . . 10
2 Different scenarios where the process model and process and measurement noise covariance
term are in noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 5/28
List of Algorithms
1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2 Unscented Kalman Filter (UKF)- Additive (zero-mean) noise case . . . . . . . . . . . . . . . 19
5
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 6/28
1 Answer for Question 1
The continuous-time state-space description of a system is given below;x1
x2
=
−0.9 0
0.9 −2.1
x1
x2
+
1.0
0
u +
0.5
0
ω1 +
0
0.6
ω2 (1)
y =
0 1 x1
x2
+ ν (2)
The system and measurement noise terms are zero-mean, white-noise sequences with variances and co-
variances given as: E {ωω } = diag(0.2, 0.2), E {νν } = 0.1, E {ων } = 0.
(a) When obtain a discrete-time realization for the continuous-time LTI system given by (1) and (2) using
Matlab function “c2d” . Code used to generate discrete-time model is given below.
function [A,B,C,D,w1,w2]=systemmdl
%Contimuous time state space model
a=[−0.9 0;0.9 −2.1];
b=[1 ;0 ];
c=[0 1];
d=0;
sys=ss(a,b,c,d);
%Continuous to discreate
sysd=c2d(sys,0.2);
%Extract individual matrixes
A=sysd(1,1).a; %State matrix
B=sysd(1,1).b; %Input matrix
C=sysd(1,1).c; %Output matrixD=sysd(1,1).d; %Direct transmission matrix
w1=[0.5;0]; %First measurement noise
w2=[0;0.6]; %Second measurement noise
Returned discrete-time system matrixes are;
A =
0.8353 0
0.1337 0.6570
B = 0.1830
0.0148
C =
0 1
D = 0
W 1 =
0.5
0
W 2 =
0
0.6
6
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 7/28
where
xk+1 = Axk + Buk + W 1ω1 + W 2ω2 (3)
yk+1 = Cxk + Duk (4)
(b) Design a “Kalman Filter” for this system
Algorithm [1]
Algorithm 1 Kalman Filter
Initialize with:
x◦ = E [x◦]
P ◦ = E [(x◦ − x◦)(x◦ − x◦)T ]
Qn = E [(ω − ω)(ω − ω)T ] a
Rn = E [(ν − ν )(ν − ν )T ] b
for k ∈ {1,...,∞} do
Time update:
(1) Project the state ahead
x−k = Axk−1 + Buk
(2) Project the error covariance ahead
P −k = AP k−1AT + Qn
Measurement update:
(1) Compute the Kalman gain
K k = P −k H T (HP −k H T + Rn)−1
(2) Update estimate with measurement zk
xk = x−k + K k(zk −H x−
k )
(3) Update the error covariance
P k = (I −K kH )P −kend for
a For additive (zero-mean) noise case ω = 0. Hence
Qn = E [ωωT ]bFor additive (zero-mean) noise case ν = 0. Hence Rn =
E [νν T ]
Main function
clear all;
close all;
duration=100;
dt=1;
N=duration/dt;
7
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 8/28
[A,B,C,D,w1,w2]=systemmdl; % Generate discreate time stste space model for ssystem
%Process noise covariance E{ww'}
Q=0.2*eye(2);
Qn=1*Q;
%Measurement noise covariance E{vv'}
R=0.1;
Rn=1*R;
%Noise covariance E{wv'}
Nn=0;
%Initialcondition
x=[0;0];
x hat=x;
P=10*eye(2); %Initial guess for error covariance
%Variables to store data
state1actual=[];
state1estimated=[];
state1filtered=[];
state2actual=[];
state2estimated=[];
state2filtered=[];
% Inputs are constant flowrate with random fluctuations
u(1,:)=[1*ones(N/4,1);3.5*ones(N/4,1);3*ones(N/4,1);1*ones(N/4+1,1)]*1 ;
for i=1:dt:duration
%Generate Noise
LL=chol(Q); %SD of process noise
L=chol(R); %SD of measurement noise
%Proces model with noise
x=A*x+B*u(1,i)+LL*[0.5*randn;0.6*randn]; %State vector
y=C*x+L*randn; %Output vector
%Call for Kalman estimator
[x hat ,J,P]=kf(A,B,C,D,u(1,i),y,x hat ,P,Qn,Rn);
%Save date to plot
state1actual=[state1actual;x(1)];
state1estimated=[state1estimated;J(1)];
state1filtered=[state1filtered;x hat(1)];
state2actual=[state2actual;x(2)];
state2estimated=[state2estimated;J(2)];
state2filtered=[state2filtered;x hat(2)];
end
%Plot results%Time axis
t=1:dt:duration;
t=t';
%Plot state 1
figure(1)
plot(t,state1actual,'−−k',t,state1estimated,'−r',t,state1filtered,'−.b');
grid on
title('Comparison of actual, estimated and filtered value for X(1)')
xlabel('Time')
8
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 9/28
ylabel('Magnitude')
legend('Actual value','Estimated Value','Filtered Value',0)
figure(2)
plot(t,state2actual,'−−k',t,state2estimated,'−r',t,state2filtered,'−.b');
grid on
title('Comparison of actual, estimated and filtered value for X(1)')
xlabel('Time')
ylabel('Magnitude')
legend('Actual value','Estimated Value','Filtered Value',0)
Kalman Filter
function [x h at,J,P]=kf(A,B,C,D,u,y,x hat ,P,Qn,Rn)
%Prdiction level ("Time update")
J=A*x hat+B*u; %Project the state ahead
S=A*P*A'+Qn; %Project the error covariance ahead
%correction level ("Measurement update")
K=S*C'*inv(C*S*C'+Rn); % Compute the kalman gain
x hat=J+K*(y−C*J); % Update estimation with measurement
P=(eye(size(K,1))−K*C)*S; % Update the error covariance
Results
0 20 40 60 80 1000
0.5
1
1.5
2
2.5
3
3.5
4
4.5
Time
M a g n i t u d e
Actual valueEstimated Value
Filtered Value
Figure 1: Comparison of actual, estimated and filtered value for X(1)
9
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 10/28
0 20 40 60 80 100−1
−0.5
0
0.5
1
1.5
2
2.5
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
Figure 2: Comparison of actual, estimated and filtered value for X(2)
In this simulation, ratio between Qn and Rn was 2. It is not a great ratio. Moreover neither Qn
nor Rn is much closer to zero (0) compare with each other. Hence, Kalman filter does not bias
(trust) too much towards to process or measurement. It will take both estimate and measurement
for consideration and bias little towards to prediction as depicted in Figure 1 and Figure 2 since
process noise covariance comparatively higher than measurement noise.
(c) If the process and noise variances are not exactly known, we can simulate five scenarios as given in Table
1 in order to evaluate the filtering properties of Kalman Filter.
Table 1: Different scenarios where the process and noise covariance term are in noise
Scenarios (i) (ii) (iii) (iv) (v)
Qn 2000 0.0002 0.2 0.2 0.2Rn 0.1 0.1 1000 0.0001 0.2
Ratio = Qn
Rn20000 0.00002 0.00002 20000 1
In the first scenario, process noise has higher covariance compare to measurement covariance. Hence,
Kalman filter trust measurements more than process. However from (2), we can see state one (x1) is
not measured. Hence, there will not be considerable correction to estimated (predicted) state of x1.
As a result more correction applies on state x2 while less correction applies on state x1. As depicted
10
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 11/28
in Figure 3(b) filtered state for x2 overlap with actual state x2 and as in Figure 3(a) filtered state x1
bias to estimated x1 since there is no any measurements on state x1. Kalman gain for two states are
K x1 = 0.3142 and K x2 = 1.0000.
0 20 40 60 80 100−60
−40
−20
0
20
40
60
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1
0 10 20 30 40 50 60 70 80 90 100−100
−80
−60
−40
−20
0
20
40
60
80
100
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2
Figure 3: Comparison of actual, estimated and filtered value when Qn=2000 and Rn=0.1
In the second scenario, process noise has very lover covariance compared with measurement noise
covariance. Hence Kalman filter trust more on process. Moreover both the states of the system are
observable. Hence, filtered output will overlap with predicted (estimated) states as depicted in Figure
4. Kalman gain for two states are K x1 = 0.0016 and K x2 = 0.0042.
0 20 40 60 80 100−2
−1
0
1
2
3
4
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1
0 10 20 30 40 50 60 70 80 90 100−0.5
0
0.5
1
1.5
2
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2
Figure 4: Comparison of actual, estimated and filtered value when Qn
=0.0002 and Rn
=0.1
As given in Table 1 ratio between Qn and Rn is constant in second and third scenarios. Hence filtering
property of Kalman filter should be identical for both case that, filtered state should overlap with
prediction since process noise has very poor covariance than measurement noise covariance. This
observation is depicted in Figure 5. Since Kalman filter has less faith on measurements, Kalman gain
must be small for both states. Kalman gain got from simulation for two states are K x1 = 1.6353e− 04
and K x2 = 4.2296e− 04.
11
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 12/28
0 20 40 60 80 1000
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1
0 20 40 60 80 100−0.5
0
0.5
1
1.5
2
2.5
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2
Figure 5: Comparison of actual, estimated and filtered value when Qn=0.2 and Rn=1000
Similarly, ratio between Qn and Rn is constant in first and fourth scenarios. Hence filtering property of Kalman filter should be identical for both case that, filtered value should overlap with actual value for
state x2 and filtered value should bias to predicted value for state x1. The result got from simulation is
depicted in Figure 6 and corresponding Kalman gain for two states are K x1 = 0.3141 and K x2 = 0.9995.
0 20 40 60 80 1000
0.5
1
1.5
2
2.5
3
3.5
4
4.5
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1
0 20 40 60 80 100−0.5
0
0.5
1
1.5
2
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2
Figure 6: Comparison of actual, estimated and filtered value when Qn=0.2 and Rn=0.0001
Final scenario is both process and measurement noise covariances are same. In this case, filtered
value will not follow neither estimation not the actual value. However given system does not have any
measurement for first state (x1). Hence, filtered value for first state (x1) bias to estimation value than
actual values. For second state, filtered value lie between actual and predicted. Results are given in
Figure 7. Kalman gain for two states are K x1 = 0.1895 and K x2 = 0.5716.
12
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 13/28
0 20 40 60 80 1000
0.5
1
1.5
2
2.5
3
3.5
4
4.5
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1
0 20 40 60 80 100−0.5
0
0.5
1
1.5
2
2.5
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2
Figure 7: Comparison of actual, estimated and filtered value when Qn=0.1 and Rn=0.1
(d) In order to evaluate the performance of Kalman filter when both the plant model and the process andmeasurement noise covariance terms are in error, three scenarios take in to consideration.
Table 2: Different scenarios where the process model and process and measurement noise covariance term
are in noise
Scenario (i) (ii) (iii)
A [-0.8 1 ; 1.2 -2.3] [-1.5 1 ; -1.2 -0.3] [-0.1 -1 ; 1.2 0]
Qn 0.2 0.2 0.1 0.2 0.2 0.1 0.2 0.2 0.1
Rn 1000 0.0001 0.1 1000 0.0001 0.1 1000 0.0001 0.1
From the tuning parameters (qn and Rn) given in Table 2 most appropriate parameters are Qn = 0.2
and Rn = 0.0001 since process should have lower trust compare to measurement. By comparing sub
figures in Figure 8, 9 and 10 with corresponding sub figures in part (c), we can conclude that Kalman
filter does better job even both plant model and the process plus noise covariance terms are in error.
13
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 14/28
0 20 40 60 80 1000
2
4
6
8
10
12
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1, Qn=0.2 and Rn=1000, K x1 = 8.8343e− 4
0 20 40 60 80 100−1
0
1
2
3
4
5
6
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2, Qn=0.2 and Rn=1000, K x2 = 7.9624e− 4
0 20 40 60 80 100−2
0
2
4
6
8
10
12
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(c) State 1, Qn=0.2 and Rn=0.0001, K x1 = 0.4514
0 20 40 60 80 100−1
0
1
2
3
4
5
6
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(d) State 2, Qn=0.2 and Rn=0.0001, K x2 = 0 .9995
0 20 40 60 80 100−2
0
2
4
6
8
10
12
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(e) State 1, Qn=0.1 and Rn=0.1, K x1 = 0 .3239
0 20 40 60 80 100−1
0
1
2
3
4
5
6
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(f) State 2, Qn=0.1 and Rn=0.1, K x2 = 0.5889
Figure 8: Comparison of actual, estimated and filtered value when A=[-0.8 1 ; 1.2 -2.3]
14
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 15/28
0 20 40 60 80 100−1
−0.5
0
0.5
1
1.5
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1, Qn=0.2 and Rn=1000, K x1 = 1.9107e− 4
0 20 40 60 80 100−3.5
−3
−2.5
−2
−1.5
−1
−0.5
0
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2, Qn=0.2 and Rn=1000, K x2 = 9.9772e− 4
0 20 40 60 80 100−1
−0.5
0
0.5
1
1.5
2
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(c) State 1, Qn=0.2 and Rn=0.0001, K x1 = −0.2578
0 20 40 60 80 100−3.5
−3
−2.5
−2
−1.5
−1
−0.5
0
0.5
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(d) State 2, Qn=0.2 and Rn=0.0001, K x2 = 0 .9995
0 20 40 60 80 100−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
1.2
1.4
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(e) State 1, Qn=0.1 and Rn=0.1, K x1 = 0.− 0.0973
0 20 40 60 80 100−3.5
−3
−2.5
−2
−1.5
−1
−0.5
0
0.5
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(f) State 2, Qn=0.1 and Rn=0.1, K x2 = 0.6217
Figure 9: Comparison of actual, estimated and filtered value when A=[-1.5 1 ; -1.2 -0.3]
15
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 16/28
0 20 40 60 80 100−2.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1, Qn=0.2 and Rn=1000, K x1 = −2.4332e− 4
0 20 40 60 80 100−1
0
1
2
3
4
5
6
7
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2, Qn=0.2 and Rn=1000, K x2 = 0.0091
0 20 40 60 80 100−4
−3
−2
−1
0
1
2
3
4
5
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(c) State 1, Qn=0.2 and Rn=0.0001, K x1 = 0.7355
0 20 40 60 80 100−4
−2
0
2
4
6
8
10
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(d) State 2, Qn=0.2 and Rn=0.0001, K x2 = 0 .9996
0 20 40 60 80 100−5
−4
−3
−2
−1
0
1
2
3
4
5
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(e) State 1, Qn=0.1 and Rn=0.1, K x1 = 0 .4014
0 20 40 60 80 100−4
−2
0
2
4
6
8
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(f) State 2, Qn=0.1 and Rn=0.1, K x2 = 0.6749
Figure 10: Comparison of actual, estimated and filtered value when A=[-0.1 -1 ; 1.2 0]
16
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 17/28
(e) State transition matrix (A) of this plant is a 2 × 2 matrix. Hence observability matrix is;
O =
C
CA
(5)
O = 0 1
0.9 −2.1
(6)
rank(O) = 2 (7)
From (7), rank of the system is equal to number of state variable of the plant. Hence, system is fully
observable. When we change a11 = 0, state transition matrix becomes;
A =
0 0
0.9 −2.1
However, this modification does not make any change to observability matrix. It still keep as (6)
and rank of the O equal to 2. Hence, still both the states are observable. In order to make system
unobservable, assume a21 = 0. Then state transition matrix becomes;
A =
0.9 0
0 −2.1
and observability matrix becomes;
O =
0 1
0 −2.1
(8)
rank of (8) is one which implies states are not fully observable. When a state become unobservable
Kalman filter does not apply any correction to that state estimation at the state update step. However,
this unobservable state does not effect to the Kalman filter performance on the observable states.
Hence filtering capability of observable state keep as it is. For this example, state one (x1) becomesunobservable after the modification and Kalmann gain for that state keep as zero for all simulation
attempt as depicted in Figure 11.
17
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 18/28
0 20 40 60 80 1000
1
2
3
4
5
6
7
8
9x 10
7
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(a) State 1, Qn=0.2 and Rn=1000, K x1 = 0
0 20 40 60 80 100−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
Time
M a g
n i t u d e
Actual value
Estimated Value
Filtered Value
(b) State 2, Qn=0.2 and Rn=1000, K x2 = 3.5172e− 4
0 20 40 60 80 1000
1
2
3
4
5
6
7
8x 10
7
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(c) State 1, Qn=0.2 and Rn=0.0001, K x1 = 0
0 20 40 60 80 100−1.2
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(d) State 2, Qn=0.2 and Rn=0.0001, K x2 = 0 .9995
0 20 40 60 80 1000
1
2
3
4
5
6
7
8 x 10
7
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(e) State 1, Qn=0.1 and Rn=0.1, K x1 = 0
0 20 40 60 80 100−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
Time
M a g n i t u d e
Actual value
Estimated Value
Filtered Value
(f) State 2, Qn=0.1 and Rn=0.1, K x2 = 0.5533
Figure 11: Comparison of actual, estimated and filtered value when a plat (state variables) is(are) not fully
observable
18
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 19/28
2 Answer for Question 2
Algorithm [2]
Algorithm 2 Unscented Kalman Filter (UKF)- Additive (zero-
mean) noise caseInitialize with:
x◦ = E [x◦]
P ◦ = E [(x◦ − x◦)(x◦ − x◦)T ]
for k ∈ {1, ...,∞} do
Calculate sigma points:
χk−1 =xk−1 xk−1 + γ
P k−1 xk−1 − γ
P k−1
Time Update:
χk|k−1 = F [χk−1, uk−1]
x−k =
2L
i=0W
(m)i χi,k|k−1
P −k =2Li=0
W (c)i
χi,k|k−1 − x−
k
χi,k|k−1 − x−
k
T + Rv
a χk|k−1 =
x−k x−k + γ
P −k x−k − γ
P −k
Y k|k−1 = H χk|k−1
y−k =
2Li=0
W (m)i Y i,k|k−1
Measurement Update:
P ykyk =
2L
i=0W
(c)i
Y i,k|k−1 − y−k Y i,k|k−1 − y−k
T + Rn
P xkyk =2Li=0
W (c)i
χi,k|k−1 − x−
k
Y i,k|k−1 − y−k
T Kk = P xkykP −1ykyk
xk = x−k +Kk
yk − y−k
P k = P −k −KkP ykykK
T k = P −k −KkP xkyk
end forwhere, γ =
√ L + λ, λ=composite scaling parameter, L=dimension of the state, Rv=process
noise covariance, Rn=measurement noise covariance, W i=weights as below;
Weights Calculation:
χ◦ = x
χi = x +
(L + λ)P xi
i = 1, ...,L
χi = x− (L + λ)P xi−L
i = L + 1, ...,2L
W (m)◦ = λ
(L+λ)
W (c)◦ = λ
(L+λ)+(1−α2+β)
W (m)i = W
(c)i = 1
2(L+λ)i = 1, ...,2L
where, λ = α2(L + κ) − L is a scaling parameter. α determines the spread of the sigma
points around x and is usually set to a small positive value (e.g., 1e − 3). κ is a secondary
scaling parameter which is usually set to 0, and β is used to incorporate prior knowledge of
the distribution of x (for Gaussian distributions, β = 2 is optimal)
a Here we have to redraw a new set of sigma points to incorporate the effect
of the additive process noise.
19
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 20/28
Implementation: System Dynamic In order to implement UKF, we consider the non-linear system given
in Figure 12 (4-tank model).
Figure 12: Four tank system
Continuous-time dynamic equations that govern the system is given in (9) [3].
dh1
dt= −
a1
A1
2gh1 +
a3
A1
2gh3 +
γ 1k1
A1u1
dh2
dt= −
a2
A2
2gh2 +
a4
A2
2gh4 +
γ 2k2
A2u2
dh3
dt = −
a3
A3
2gh3 +
(1− γ 2)
A3 k2u2 (9)dh4
dt= −
a4
A4
2gh4 +
(1− γ 1)
A3k1u1
Moreover the output of the system is given by (10).
y1 = h1
y2 = h2 (10)
First, system dynamic given by (9) converted in to discrete time model using finite difference approxi-
mation given in (11).
dh
dt≈
hk+1 − hk
δt(11)
20
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 21/28
The resulting discrete time system dynamics model is given in (12)
h1(k + 1) = h1(k) −a1
A1
2gh1δt +
a3
A1
2gh3δt +
γ 1k1
A1u1δt
h2(k + 1) = h2(k) −a2
A2 2gh2δt +
a4
A2 2gh4δt +
γ 2k2
A2u2δt
h3(k + 1) = h3(k) − a3A3
2gh3δt + (1− γ 2)
A3k2u2δt (12)
h4(k + 1) = h4(k) −a4
A4
2gh4δt +
(1− γ 1)
A3k1u1δt
Moreover process noise covariance becomes Q = (δt)2Q.
Implementation: Matlab Code [4]
– Main Function
clear all;
close all;
n=4; %number of state
duration=500; %Data Generation
global dt;
dt=0.1;
N=duration/dt;
Q=(dtˆ2)*0.002*eye(4); % Covariance of process noise
R=0.02*eye(2); % Covariance of measurement noise
q=chol(Q); % std of process
r=chol(R); % std of measurement
[A, B, C, D]=systemmdl2; % Getting the coefficient for the process
% Inputs are constant flowrate with random fluctuations
u(1,:)=[1*ones(N/4,1);3.5*ones(N/4,1);3*ones(N/4,1);1*ones(N/4+1,1)]*1 ;
u(2,:)=[1*ones(N/4,1);3.5*ones(N/4,1);3*ones(N/4,1);1*ones(N/4+1,1)]*1 ;
h(:,1)= [14.1 11.2 7.2 4.7]'; %Initializtion State
y(:,1)=C*h(:,1);
%Tuning Parameters
Qf=1*Q;
Rf=1*R;
% Initialisation Block
P0 = 10*eye(4); % Error Covariance of initial states
P=P0;
x initial= [14.1 11.2 7.2 4.7]*1; % Initialising for UKF
in=u(:,1);
f1=@(x,in)[x+A*sqrt(x)+B*in]; % nonlinear state equations
f2=@(x,in)[C*x+D*in]; % measurement equation
21
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 22/28
s=x initial'; % initial state
x=s+q*randn(4,1); % initial state with noise
% allocate memory
xV = zeros(n,N); %Filtered states
hf1=[];
hf2=[];
hf3=[];
hf4=[];
%Actual ststes
ha1=[];
ha2=[];
ha3=[];
ha4=[];
%Output (Measurements)
ho1=[];
ho2=[];
for i=1:N
h=h+A*sqrt(h)+B*u(:,i)+q*randn(4,1);
z=C*h+r*randn(2,1);
s = f1(s,u(:,i)) + q*randn(4,1); % update process
[x, P] = ukf(f1,x,u(:,i),P,f2,z,Q,R); % ukf
% save filtered estimations
hf1=[hf1 x(1,1)];
hf2=[hf2 x(2,1)];
hf3=[hf3 x(3,1)];
hf4=[hf4 x(4,1)];
% Save actual states
ha1=[ha1 s(1,1)];
ha2=[ha2 s(2,1)];
ha3=[ha3 s(3,1)];
ha4=[ha4 s(4,1)];
% Save (output) measurment
ho1=[ho1 z(1,1)];
ho2=[ho2 z(2,1)];
end
%Plot Results
figure(1)
plot(1:N, ha1, '−k', 1:N, hf1, '−.k');
grid on
title('Comparison of actual and filtered value for tank 1')
xlabel('Dynamic Steps')
ylabel('Fluid Level (cm)')
legend('Actual value','Filtered Estimation',0)
figure(2)
plot(1:N, ha2, '−k', 1:N, hf2, '−.k');
grid on
title('Comparison of actual and filtered value for tank 2')
xlabel('Dynamic Steps')
ylabel('Fluid Level (cm)')
legend('Actual value','Filtered Estimation',0)
22
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 23/28
figure(3)
plot(1:N, ha3, '−k', 1:N, hf3, '−.k');
grid on
title('Comparison of actual and filtered value for tank 3')
xlabel('Dynamic Steps')
ylabel('Fluid Level (cm)')
legend('Actual value','Filtered Estimation',0)
figure(4)
plot(1:N, ha4, '−k', 1:N, hf4, '−.k');
grid on
title('Comparison of actual and filtered value for tank 4')
xlabel('Dynamic Steps')
ylabel('Fluid Level (cm)')
legend('Actual value','Filtered Estimation',0)
– System model coefficients
function [A, B, C, D]=systemmdl2
% Step 2 : Computing the A,B,C & D Matrices%=========================================
global dt;
%Initial values of the parameters
h1 = 25; h2 = 20 ; h 3 = 1 0; h4 = 1 0; % units in cm.
% Defining A, B, C, D Matrices.
% Parametric data
at1=0.071; at3=0.071; at2=0.057; at4=0.057; % Orifice area in cm.
At1=28; At3=28; At2=32; At4=32; % Tank area in cm.
kc=0.5; % Units are in Volts/cm.
g=981; % Units in cm./sec.sq.
nu1=0.7; nu2=0.6; % Percentage opening of the splitter valvekt1=3.33; kt2=3.35; % Units are in cc/Volt−seconds
T1 = (at1*dt/At1) * sqrt(2*g);
T2 = (at2*dt/At2) * sqrt(2*g);
T3 = (at3*dt/At3) * sqrt(2*g);
T4 = (at4*dt/At4) * sqrt(2*g);
a11 = −T1; a12 = 0; a13 = (At1 * T3)/At3; a14 = 0;
a21 = 0; a22 = −T2; a23 = 0; a24 = (At2 * T4)/At4;
a31 = 0; a32 = 0; a33 = −T3; a34 = 0;
a41 = 0; a42 = 0; a43 = 0; a44 = −T4;
A = [a11 a12 a13 a14; % The 'A' Matrix
a21 a22 a23 a24;
a31 a32 a33 a34;
a41 a42 a43 a44];
b11 = (nu1 * kt1*dt)/At1; b12 = 0;
b21 = 0; b22 = (nu2 * kt2*dt)/At2;
b31 = 0; b32 = ((1 − nu2)* kt2*dt)/At3;
b41 = ((1 − nu1)* kt1*dt )/At4 ; b42 = 0;
23
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 24/28
B = [b11 b12; % The 'B' Matrix
b21 b22;
b31 b32;
b41 b42];
%
c11 = k c; c 12 = 0; c13 = 0 ; c 14 = 0 ;
c21 = 0 ; c22 = kc; c23 = 0 ; c 24 = 0 ;
C = [c11 c12 c13 c14; %The 'C' Matrix
c21 c22 c23 c24];
D=0;
– UKF main function
function [x,P]=ukf(fstate,x,u1,P,hmeas,z,Q,R)
%Parameters and constant to calculate sigma points
%Parameter list
L=numel(x); %numer of states
m=numel(z); %numer of measurements
%Constant listalpha=1e−3; %default, tunable
ki=0; %default, tunable
beta=2; %default, tunable
%compute sigma point
lambda=alphaˆ2*(L+ki)−L; %scaling factor
c=L+lambda; %scaling factor
C=sqrt(c);
%************************ Weight calculation ******************************
Wm=[lambda/c 0.5/c+zeros(1,2*L)]; %Weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1−alphaˆ2+beta); %Weights for covariance
%************************ Time update *************************************
% Sigma point calculation
A = C*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [ x Y + A Y−A]; % Sigma points
% Unscented transformation
[x1,X1,P1,X2]=ut(fstate,X,u1,Wm,Wc,L,Q); % Process
[z1,Z1,P2,Z2]=ut(hmeas,X1,u1,Wm,Wc,m,R); % Measurments
Pxx=P1; % Project error covariance ahead
%*********************** Measurement update *******************************
Pxy=X2*diag(Wc)*Z2'; % Transformed cross−covariance
Pyy=P2; % Transformed auto−covariance
K=Pxy*inv(Pyy); % Kalman gainx=x1+K*(z−z1); % Update estimate with measurement
P=Pxx−K*Pxy'; % Update the error covariance
– Unscented transformation
function [y,Y,P,Y1]=ut(f,X,u1,Wm,Wc,n,R)
L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
24
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 25/28
for k=1:L
Y(:,k)=f(X(:,k),u1); % Unscented transformation
y=y+Wm(k)*Y(:,k); % weighted sum to calculate mean state or measurement
end
Y1=Y−y(:,ones(1,L)); % Error
P=Y1*diag(Wc)*Y1'+R; % Projet the error covariance to ahead (weighted sum).
Results
0 1000 2000 3000 4000 50000
5
10
15
Dynamic Steps
F l u i d L e v e l ( c m )
Actual value
Filtered Estimation
(a) Tank 1
0 1000 2000 3000 4000 50002
4
6
8
10
12
14
Dynamic Steps
F l u i d L e v e l ( c m )
Actual value
Filtered Estimation
(b) Tank 2
0 1000 2000 3000 4000 50000
1
2
3
4
5
6
7
8
9
Dynamic Steps
F
l u i d L e v e l ( c m )
Actual value
Filtered Estimation
(c) Tank 3
0 1000 2000 3000 4000 50000
1
2
3
4
5
6
Dynamic Steps
F
l u i d L e v e l ( c m )
Actual value
Filtered Estimation
(d) Tank 4
Figure 13: Comparison of actual and filtered value for four tank system
25
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 26/28
2.1 Comparison of implementation of KF (Algorithm 1) and UKF (Algorithm
2)
Initialization of two implementation is identical to each other
In KF at the time updates, it project the state ahead as first step. But UKF does not project state as
a single unit. UKF, first extracted several point (generally three points) call sigma point from currentprocess state distribution and project all sigma point to next state using nonlinear state transition
equation. We should be much careful about matrix dimensions when we do sigma point calculation. In
sigma point calculation equation has a part xk−1±γ
P k−1, since process has four state variables P k−1
would be squire matrix of 4 × 4. After do Cholesky decomposition and scale, it will remain as 4 × 4
squire matrix. On the other hand, xk−1 is a 4 × 1 vector. Hence, xk−1 ± γ
P k−1 cannot be achieved.
To overcome this issue we generated 4 × 4 matrix duplicating xk−1 vector four times as given bellow.
function X=sigmas(x,P,c)
A = c*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [ x Y + A Y−A];
Because of this modification selection of sigma points always align with principle axis. Hence difference
of the first step is KF “project the state ahead” while UKF “project the sigma points ahead”.
Second step of time update is “Project the error covariance ahead”. IN KF (13) used to project the
error covariance. If we expand the right hand side of the (13), AE {(xk−1|k−1 − xk−1|k−1)(xk−1|k−1 −
xk−1|k−1)T }AT + Qn which has summation of error covariance of state prediction and process noise
covariance. Same thing take place in UKF. First it calculate mean of the projected sigma points using
x−k =
2Li=0
W (m)i χi,k|k−1. Then error covariance projection takes place using (14). In here projected
process error covariance is the weighted sum of error covariance of individual sigma points. Finally it
adds process error noise covariance as KF.
P −k = AP k−1AT + Qn (13)
P −k =
2Li=0
W (c)i
χi,k|k−1 − x−
k
χi,k|k−1 − x−
k
T + Qn (14)
Noise covariance has added in both cases. Major difference is part AP k−1AT in KF has replaced by
weighted sum term2Li=0
W (c)i
χi,k|k−1 − x−
k
χi,k|k−1 − x−
k
T since UKF does not have state transition
matrix corresponds to linear state instead it has sigma points to represent nonlinear state transition.
Hence, in UKF takes weighted sum instead of direct multiplication with error covariance of previous
state.
Then UKF projects the measurement to ahead similar way it projects the state ahead. And calculate
the mean of projected measurements. Moreover, at each time update UKF drawn new set of sigma
points which is not done in KF.
Measurement updates has three steps in both KF and UKF.
– Compute the Kalman gain: In KF, Kalman gain is calculated using (15). P −k H T is the cross-
covariance of measurement and process error and HP −k H T +Rn is the covariance of measurements.
26
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 27/28
In UKF Kalman gain is calculated using (16). P xkyk is the cross-covariance of measurement and
process error and P ykyk is the covariance of measurements. Hence calculation of Kalman gain
is identical in two algorithm. Only difference is cross-covariance and covariance in KF calcu-
lated using simple matrix multiplication while UKF uses weighted sum of individual sigma points
covariance as given in (17) and (18).
K k = P −k H T (HP −k H T + Rn)−1 (15)
Kk = P xkykP −1ykyk(16)
P ykyk =
2Li=0
W (c)i
Y i,k|k−1 − y−k
Y i,k|k−1 − y−k
T + Rn (17)
P xkyk =2Li=0
W (c)i
χi,k|k−1 − x−
k
Y i,k|k−1 − y−k
T (18)
– Update estimation with measurement is identical in each algorithm.
– Error covariance update is also identical to each other.
27
8/2/2019 ENGR 9826 Ass2 Report
http://slidepdf.com/reader/full/engr-9826-ass2-report 28/28
Bibliography
[1] G. Welch and G. Bishop, “An introduction to the kalman filter,” University Lecture,University of North
Carolina, USA, 2001, accesed on March 1, 2012.
[2] E. A. Wan and R. van der Merwe, “The unscented kalman filter,” Online:
http//www.stomach.v2.nl/docs/TechPubs/Tracking and AR/wan01unscented.pdf, accesed on March 1,
2012.
[3] I. Syed, “State estimation: Kalman filter and beyond,” University Lecture,Memorial University of New-foundland, Canada, 2012.
[4] Y. Cao, “Learning the unscented kalman filter,” Online: http//www.mathworks.com/matlabcentral/fileexchange/
Jan 2008, accesed on March 1, 2012.
top related