engr 9826 ass2 report

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



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



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



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



List of Algorithms

1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2 Unscented Kalman Filter (UKF)- Additive (zero-mean) noise case . . . . . . . . . . . . . . . 19

5



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



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



[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



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



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



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



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



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



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



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


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


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


Figure 9: Comparison of actual, estimated and filtered value when A=[-1.5 1 ; -1.2 -0.3]

15



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


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


Figure 10: Comparison of actual, estimated and filtered value when A=[-0.1 -1 ; 1.2 0]

16



(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



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


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


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


Figure 11: Comparison of actual, estimated and filtered value when a plat (state variables) is(are) not fully

observable

18



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



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



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



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



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





22



figure(3)

plot(1:N, ha3, '−k', 1:N, hf3, '−.k');

grid on





figure(4)

plot(1:N, ha4, '−k', 1:N, hf4, '−.k');

grid on





– 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);




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



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



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



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



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



T + Rn (17)

P xkyk =2Li=0

W (c)i

χi,k|k−1 − x−

k


T (18)

– Update estimation with measurement is identical in each algorithm.

– Error covariance update is also identical to each other.

27



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.

engr 9826 ass2 report

Documents