$30.00
Description
Problem set due Thursday, March 15, 8:00PM. Submit a pdf le (a neat scan of your handwritten solution) via bCourses. The copier on 6th oor of Etcheverry is available 24/7, and can be used without a login to email scans to any berkeley.edu address.

Generalizing (and programing) the batchestimation: In class (and the notes) we derived the batch
formulation of Kalman ltering, generating the correct matrices to compute L(x_{k}jy_{0}; : : : ; y_{k} _{1}): It was mentioned that we could also improve our estimates of previous states as new information (the y values) are presented, for example
L(x_{0}jy_{0}; : : : ; y_{k} _{1}); or L(x_{k}jy_{0}; : : : ; y_{k} _{1}; y_{k})
In this problem, we introduce the appropriate matrices (and some recursions to make building the batch matrices easier) to compute

02
x_{1}
3
y
0
1
^{x}0
2
y
1
3
.
B6
_{.}.
7
C
L
x
6
.^{.}
B6
7
y
7C
B6
k 1
7
6
7C
B6
x
7
6
k
1
7C
B6
k
7
4
_{5}C
@4
5
A
Remark: You can easily modify the ideas below to get batch solutions to

02
x_{1}
3
2
y_{1}
31
x_{0}
y_{0}
L
.
.
B6
7
6
7C
B6
x
7
6
y
7C
B6
j
7
6
k
7C
@4
5
4
5A
for any j and k. Although we did not do it in class, there are recursive formulations ( xed amount of computation each step) to compute x^_{jjk} for any j and k. Recall that in lecture, we derived recursive formula only for x^_{k} _{1jk} and x^_{kjk}. Now onto the batch generalization…
Dimensions: x_{k} 2 R^{n}x ; y_{k} 2 R^{n}y ; w_{k} 2 R^{n}y
Relevant Matrices: Note that I am using blue to distinguish the stateevolution matrix A_{k} at time k from the caligraphic A_{k;j}.

^{A}k;j ^{:=} ^{A}k
_{1} A_{j} 2 R^{n}^{x} ^{n}^{x}
k > j 0
N_{k} :=
Ak;1^{E}0 Ak;2^{E}1
Ak;3^{E}2Ak;k
1^{E}k 2 ^{E}k 1
_{2} _{R}nx knw
1 of 13
Available: March 8 Due: March 15

2
E_{0}
0
0
0
0
3
0
0
0
0
0
_{k} :=
6
A2;_{.}1^{E}0
E_{.}
1
0_{.}
_{.}
0_{.}
0_{.}
7
R
(k+1)n_{x}
kn_{w}
6
.^{.}
.^{.}
.^{.}
. _{.}
.^{.}
.^{.}
7
2
6
7
6
k 2;1
E
0
k 2;2
E
1
k 2;3
E
2
0
0
7
6
A
A
7
6
k 1;1
E
0
A
E
1
k 1;3
E
2
E
k 2
0
7
6
A
k 1;2
A
7
6
k;1^{E}0
A
k;3^{E}2
k;k 1^{E}k 2
^{E}k 1
7
6
A
k;2^{E}1
A
7
4
A
A
5

3

I
2
C_{1}
1;0
3
^{A}2;0
6
1;0
7
6
C_{0}
7
A
_{k} :=
A_{.}
R
(k+1)n_{x}
n_{x}
; R_{k} :=
C_{2}
2;0
R
kn_{y}
n_{x}
6
.^{.}
7
2
^{A}.
2
6
7
6
.^{.}
7
6
7
6
7
6
A
k
2;0
7
6
C
7
6
7
6
C
k 2
A
k 2;0
7
6
A
k
1;0
7
6
7
6
k;0
7
6
k 1
A
k 1;0
7
6
A
7
4
5
4
5

3

x_{0}
2
^{w}1
3
2
^{y}1
3
x_{2}
6
x_{1}
7
6
^{w}0
7
6
^{y}0
7
_{k} :=
.
R
(k+1)n_{x}
k 1
:=
^{w}2
R
kn_{w}
k 1
:=
^{y}2
R
kn_{y}
X
6
.^{.}
7
2
W
.
2
Y
.
2
6
x
7
6
.^{.}
7
6
.^{.}
7
6
7
6
7
6
7
6
k
2
7
6
w
7
6
y
7
6
x
7
6
w
k 2
7
6
y
k 2
7
6
k
1
7
6
k 1
7
6
k 1
7
6
^{x}k
7
6
7
6
7
6
7
4
5
4
5
4
5
2
^{C}1^{E}0
^{F}1
0
0
0
3
6
^{F}0
0
0
0
0
7
S_{k} :=
^{C}2^{A}_{.}1^{E}0
^{C}2_{.}^{E}1
^{F}_{.}2
_{.}
0_{.}
0_{.}
2
_{R}kny knw
6
_{.}.
_{.}.
_{.}.
. _{.}
_{.}.
_{.}.
7
6
7
6
C
k 2 k 2;1
E
0
C
k 2 k 2;2
E
1
C
k 2;3
E
2
F
k 2
0
7
6
k 2
A
7
6
C
A
E
0
C
A
E
C
k 1;3
E
2
C
k 1
E
k 2
F
k 1
7
6
k 1 k 1;1
k 1 k 1;2
1
k 1
A
7
4
A
A
5
Initializations: Recall that Matlab can handle empty arrays with some nonzero dimensions (though at least one dimension needs to be 0 so that the array is truly empty. In the de nitions below, Iāve included, correctly, the nonzero dimension.

_{0} = empty; 0_{n}_{x} _{0};
0 ^{=} ^{I}n_{x} n_{x} ^{;}^{N}0 ^{= empty;} ^{0}n_{x} 0^{;}
S_{0} = empty; 0_{0 0};
^{R}0 ^{= empty;} ^{0}0 n_{x} ^{;}A0;0 ^{=} ^{I}n_{x} n_{x}
Recursions: for i 0

^{A}i+1;0 ^{=} ^{A}i^{A}i;0^{;}
i+1 ^{=}
i
; N_{i+1} = A_{i}N_{i} E_{i}
_{;}
^{A}i+1;0
2 of 13
Due: March 15 

i+1 
= 
^{N}i+1 
x 
w 
; R_{i+1} 
= 
C_{i}A_{i;0} 
; S_{i+1} 
= 
C_{i}N_{i} 
F_{i} 

i 
^{0}(i+1)n 
n 
R_{i} 
S_{i} 
^{0}in_{y} 
n_{w} 
For i 0, the quantities
^{fA}i+1;0^{;} i+1^{; N}i+1^{;} i+1^{; R}i+1^{; S}i+1^{g}
can be calculated (recursively) from
fA_{i;0}; _{i}; N_{i}; _{i}; R_{i}; S_{i}g and fA_{i}; E_{i}; C_{i}; F_{i}g
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
This is implemented in buildBatch.m, shown below.
begin code
function [calA,Psi,N,Gam,R,S] = buildBatch(calA,Psi,N,Gam,R,S,A,E,C,F)

Initialization call uses one input argument (state dimension)

[calA,Psi,N,Gam,R,S] = buildBatch(nX)

General recursive call uses the function definition line

[calA,Psi,N,Gam,R,S] = buildBatch(calA,Psi,N,Gam,R,S,A,E,C,F) if nargin==1
nx = calA; % first (and only) argument is NX calA = eye(nx); Psi = eye(nx); Gam = zeros(nx,0); N = zeros(nx,0); S = zeros(0,0); R = zeros(0,nx);
else


Determine k, ny, nw, nx from dimensions of A, E, C, F and (eg) S [ny,nx] = size(C);

nw = size(E,2);
k = size(S,1)/ny;


Use recursions to build new, larger matrices. Use temporary



variable for the updated calA and N, since the original values are



needed to define some of the other updated matrices.

newCalA = A*calA;
newN = [A*N E];
Psi = [Psi;newCalA];
Gam = [[Gam zeros((k+1)*nx,nw)];newN];
R = [R;C*calA];
S = [S zeros(k*ny,nw);C*N F];
calA = newCalA;
N = newN;
end
end code
Evolution equations:
^{X}k ^{=} k^{x}0 ^{+} kWk 1^{;} Yk 1 ^{=} ^{R}k^{x}0 ^{+} ^{S}kWk 1
De ne Z := X_{k}; P := Y_{k} _{1}. We are interested in L(ZjP ) and the variance of Z L (ZjP ). In general, these are
L(ZjP ) = _{Z;P P} ^{1}(P _{P }) + _{Z} ; _{Z} _{(}_{ZjP}_{ ) }= _{Z} _{Z;P} _{P }^{1} ^{T}_{Z;P}
3 of 13
Due: March 15 

Speci cally, for the Kalman lter application, the various quantities are 

_{Z} = _{k}m_{0};_{P }= R_{k}m_{0} 

and 

T 
+ 
T 

Z ^{=} k 0 _{k} 
k W k 

T 
T 

P ^{=} ^{R}k 0^{R}_{k} 
^{+} ^{S}k W ^{S}k 

T 
+ 
T 

Z;P ^{=} k 0^{R}_{k} 
k W ^{S}k 
Plugging in everything gives our optimal estimate of all states from x_{0} to x_{k}, based on all of the

measurements y_{0} to y_{k}
_{1},
R_{k} _{0}R_{k}^{T} + S_{k W} S_{k}^{T} ^{1} (Y_{k} _{1}
L(X_{k}jY_{k} _{1}) = _{k} _{0}R_{k}^{T} + _{k W} S_{k}^{T}
R_{k}m_{0}) + _{k}m_{0}
Simple regrouping
L(X_{k}jY_{k} _{1}) =
T
+
T
T
T
1
^{Y}k 1
k 0^{R}_{k}
k W ^{S}k
^{R}k 0^{R}_{k}
^{+} ^{S}k W ^{S}k

k
k 0
_{k}{z
W
k 0
+ S_{k}
}
W
k 0
h
k
_{:=L}_{k}batch
^{k} batch
k
k
1
i
+
R
T
T
R R
T
T
R m
+S
S

}
:=V^{{z}_{k}
and error variance

X_{k} (X_{k}jY_{k} _{1}) ^{=}
T
+
k 0 _{k}
k W
( _{k} _{0}R_{k}^{T} +
T 

k 

T 
T 
T 
1 
( 
T 
+ 
T 
T 

k W ^{S}_{k} ^{)(R}k 0^{R}_{k} 
^{+} ^{S}k W ^{S}_{k} ^{)} 
k 0^{R}_{k} 
k W ^{S}_{k} ^{)} 
The diagonal blockentries of (each of dimension n_{x} n_{x}) are most important, as they signify the variance (\spread”) in the error of the estimate of each timeinstance of the state.
Summarizing: for each k 1, there exist matrices L^{batch}_{k} and V_{k}^{batch} such that
L(X_{k}jY_{k} _{1}) = L^{batch}_{k}Y_{k} _{1} + V_{k}^{batch}m_{0}
1
2
3
4
5
6
7
8
9
10
11
12
13
The function batchKF calculates these, using all of the ideas presented.
begin code
function [LkBatch,VkBatch,ekVar] = …
batchKF(Amat,Emat,Cmat,Fmat,Sigma0,SigmaW,k)
nx = size(Amat,1);

Initialize the 6 batch matrices [calA,Psi,N,Gam,R,S] = buildBatch(nx);

Fill in batch matrices in order to estimate x0 to xk, as linear

combinations of y from 0,1,…,k1. This needs the statespace matrices

defined on i = [0,1,…,k1]
for i=0:k1
Ai = Amat(:,:,i+1); % i+1 is needed because Matlab indices start at 1
Ei = Emat(:,:,i+1);
Ci = Cmat(:,:,i+1);
4 of 13
Available: March 8 Due: March 15

Fi = Fmat(:,:,i+1);

[calA,Psi,N,Gam,R,S] = buildBatch(calA,Psi,N,Gam,R,S,Ai,Ei,Ci,Fi);

end

% The next line assumes that the variance of w_k is constant across k,

% given by the single matrix SigmaW. The KRON command simply repeats this

% matrix in a block diagonal form, as in the lecture slides.

SigmaBarW = kron(eye(k),SigmaW);

SigmaZ = Psi*Sigma0*Psiā + Gam*SigmaBarW*Gamā;

SigmaP = R*Sigma0*Rā + S*SigmaBarW*Sā;

SigmaZP = Psi*Sigma0*Rā + Gam*SigmaBarW*Sā;

LkBatch = SigmaZP/SigmaP;

VkBatch = Psi – Lk*R;

ekVar = SigmaZ – Lk*SigmaZPā;
end code
Here k 1 is an integer, and Amat is a 3dimensional array, of dimension [nX, nX, N], where N > k, and for each nonnegative i, Amat(:,:,i+1) equals A_{i} (note the index issue that we being our sequences with an index of 0, but Matlab begins their arrays with an index of 1).

~
batch
~
batch
, for
Finally, if L_{k}
is de ned as the last 2n_{x} rows of L_{k}
and V_{k}
as the last 2n_{x} rows of V_{k}
example with the code
begin code

Ltildek = LkBatch(end2*nX+1:end,:)

Vtildek = VkBatch(end2*nX+1:end,:)

end code
then
x^_{kjk}^{j}
1
_{= }_{L}
^{x}k
Y_{k} _{1}
= L^{~}_{k}Y_{k} _{1} + V^{~}_{k}m_{0}
k
1
^{x^}k 1
1
x_{k}
which is more related to the recursive derivation in lecture, which did not consider estimates of older states using newer measurements (eg., in lecture, we did not derive the recursion for x^_{4j9} (estimate of x_{4} based on y_{0}; y_{1}; : : : ; y_{9}).

Verify the expressions in the Recursions section, given the de nitions in the Relevant Matrices section.

Verify the expressions in the Evolution Equations section.

Examine the estimation problem solved below. The command batchKF.m is included in the assignment .zip le, and has the buildBatch code as a local function, so you do not have to create these les. Note that for k = 1; 2; : : : ; 6, we use the batchcode to get the best estimate of X_{k} from Y_{k} _{1}. The code displays certain rows of L^{batch}_{k} and V_{k}^{batch} which are speci cally relevant for the estimate x^_{k} _{1jk} _{1}
1
2
begin code
Amat = repmat(1,[1 1 20]);
nX = size(Amat,1);
5 of 13
Available: March 8 Due: March 15
3
4
5
6
7
8
9
10
11
12
Emat = repmat(0,[1 1 20]);
Cmat = repmat(1,[1 1 20]);
Fmat = repmat(1,[1 1 20]);
sX = 1000; sW = 1; m0=2;
for k=1:6
[LkBatch,qkBatch,VkBatch,eVar] = …
batchKF(Amat,Emat,Cmat,Fmat,m0,sX,sW,k); LkBatch(end2*nX+1:endnX,:) VkBatch(end2*nX+1:endnX)
end
end code
i. Interpret the estimation problem, speci cally
In the absence of process noise, how would the state evolve?
In the absence of measurement noise, how are the state and measurement related? How are the process noise, measurement noise and initial condition variances related? ii. The solution is computed for several di erent horizons. Study the \gain” matrix LkBatch and o set qkBatch and interpret how the optimal estimate is processing Y_{k} _{1}, and using
m_{0} to obtain the estimate x_{k} _{1jk} _{1}
iii. If you had to intuitively design an estimator for exactly this problem, without any Kalman ltering knowledge, given the speci c statistical properties of the initial condition, process noise, and measurement noise, would you be likely to pick a similar estimation strategy?
iv. Repeat the exercise for the following example begin code

Amat = repmat(1,[1 1 20]);

nX = size(Amat,1);

Emat = repmat(0,[1 1 20]);

Cmat = repmat(1,[1 1 20]);

Fmat = repmat(1,[1 1 20]);

sX = 0.1; sW = 5; m0=4;

for k=1:6

[LkBatch,qkBatch,VkBatch,eVar] = …
9 batchKF(Amat,Emat,Cmat,Fmat,m0,sX,sW,k);

LkBatch(end2*nX+1:endnX,:)

VkBatch(end2*nX+1:endnX)

end
end code

Enhance KF code: The code for the Kalman lter, as derived in class, is included in the assignment .zip le.

Included in the assignment .zip le is a function KF231BtoBuildOn.m with declaration line
begin code

function [xk1k,Sxk1k,xkk,Sxkk,Sykk1] = …

2
KF231BtoBuildOn(xkk1,Sxkk1,A,B,C,E,F,Swk,uk,yk)
end code
6 of 13
Available: March 8 Due: March 15
1
2
Modify the code, renaming it KF231B, to have four additional outputs, namely as begin code
function [xk1k,Sxk1k,xkk,Sxkk,Sykk1,Lk,Hk,Gk,wkk] = …
KF231B(xkk1,Sxkk1,A,B,C,E,F,Swk,uk,yk)
end code
for the matrices L_{k}; H_{k}; G_{k} and the estimate w^_{kjk} as de ned in the slides. This should only require adding one line of code to properly de ne L_{k}, and one or two lines for w^_{kjk}. If you look carefully at the existing code, you will see that H_{k} and G_{k} are already de ned. If you look at the variables which already exist, you should be able to do this task without any additional \inverses” (or the use of the backslash operator). Make sure to modify the help for KF231B.m as follows.
begin code

% Implements one step of the Kalman filter, using the notation in

% slides at the end of Estimation, Part 3. The input arguments are:

%xkk1 = xhat_{kk1}

%Sxkk1 = Sigma^x_{kk1}

%A, B, C, E, F: state matrices at time k

%Swk = variance in zeromean disturbance w_k

%uk = deterministic control input u_k

%yk = measurement y_k

% The output arguments are:

%xk1k = xhat_{k+1k}

%Sxk1k = Sigma^x_{k+1k}

%xkk = xhat_{kk}

%Sxkk = Sigma^x_{kk}

%Sykk1 = Sigma^y_{kk1}

%Lk (from last slide, Part 3) for:

16
%
xhat_{k+1k} = Ak x_{kk1} + Lk*(yk – Ck*x_{kk1})

%Hk (from fact #16), for:

18
%
xhat_{kk} = x_{kk1} + Hk*(yk – Ck*x_{kk1})

%Gk (from fact #17), for: what_{kk} = Gk*ek

%wkk = what_{kk}

% The input signals, xkk1, uk, yk may all be empty matrices, implying

% that the function will only update the error variances, and will not

% provide any state estimates (so xk1k, xkk and wkk will be returned

% empty as well).
end code

SteadyState Behavior: As mentioned in class, if the process statespace matrices (A_{k}; E_{k}; C_{k}; F_{k}) and the variance of w_{k} (which we now notate as W_{k} := E(w_{k}w_{k}^{T} )) are constant (ie., not timevarying), then (under very mild technical conditions, which we will not worry about) the gain and variance matrices in the Kalman lter converge to steadystate values, namely

lim L
lim H
lim
x
x
;
lim
x
x
k
= L;
k
= H;
kjk
=
kjk 1
=
1
k!1
k!1
k!1
k!1
Create some random data (A; E; C; F; W ), and con rm the convergence by calling KF231B in a loop, monitoring the di erences between L_{i+1} and L_{i} (and so on for the other matrices), and exiting the loop when suitable convergence is attained.
7 of 13
Available: March 8 Due: March 15
Hint: Recall that KF231B can be called in such a way that signals are not needed, and only the variances and gains are updated. Carefully read the help again if needed.

Separating signal from noise: Suppose P_{1} and P_{2} are linear systems (possibly timevarying), with known statespace models. The input to system P_{1} is v (producing output y_{1}), and the input to system P_{2} is d (producing output y_{2}). Both v and d are random sequences, with the following properties (for all k and j 6= k)
E(v_{k}) = 0; E(d_{k}) = 0;
and
E(v_{k}v_{k}^{T} ) = V_{k}; E(d_{k}d^{T}_{k} ) = D_{k}; E(v_{k}d^{T}_{k} ) = 0; E(v_{k}v_{j}^{T} ) = 0 E(d_{k}d^{T}_{j} ) = 0; E(v_{k}d^{T}_{j} ) = 0 Let y := y_{1} + y_{2}. Draw a simple block diagram, and label y_{1} as \noise” and y_{2} as \signal.”

Explain how we can use the Kalman ltering theory to estimate y_{2} from y. De ne the appropriate matrices, so that appropriate calls to KF231B along with some simple arithmetic would produce the desired estimate.
Note: In this problem we can interpret y_{2} as a meaningful signal, which is additively corrupted by noise, y_{1}, to produce a measurement y. The goal of ltering is to recover the meaningful signal (y_{2}) from the measured, \noisy” signal y. Remark: Be careful with subscripts. In the notation above, the subscript on d and v was interpreted as a timeindex, but the subscript on y_{1} and y_{2} is just referring to two di erent signals (each of which depends on time).

In the commented example below, P_{1} is a rstorder highpass lter, and P_{2} is a rstorder lowpass lter, and the separation is done using the steadystate values of the Kalman lter.
Study and run the code. Carefully examine the plots.
begin code

%% Separating Signal from Noise

% ME C231B, UC Berkeley,
3

%% Sample Time

% For this discretetime example, set TS=1. In Matlab, this

% just means an unspecified sampling time, totally within the

% context of pure discretetime systems.

TS = 1;
9

%% Create highpass filter

P1 = 0.4*tf([.5 .5],[1 0],TS);

[A1,E1,C1,F1] = ssdata(P1);

nX1 = size(A1,1); % will be 1

nW1 = size(E1,2); % will be 1
15

%% Create lowpass filter

P2 = tf(.04,[1 .96],TS);

[A2,E2,C2,F2] = ssdata(P2);

nX2 = size(A2,1); % will be 1

nW2 = size(E2,2); % will be 1
8 of 13
Available: March 8 Due: March 15
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

Bode plot of both bOpt = bodeoptions; bOpt.PhaseVisible = āoffā; bOpt.MagUnits = āabsā; bOpt.MagScale = ālogā; bOpt.FreqScale = ālinearā; bOpt.Xlim = [0 pi]; bOpt.Ylim = [1e4 2]; bodeplot(P2,ārā,P1,ākā,bOpt)

Form overall system which adds the outputs A = blkdiag(A1,A2);
E = blkdiag(E1,E2); C = [C1 C2];
F = [F1 F2];
nX = size(A,1); nY = size(C,1); nW = size(E,2);

Noise variance and initial condition variance

Keep it simple, and make everything Identity (appropriate

dimension)
SigW = eye(nW);
Sxkk1 = eye(nX);

Run several iterations to get the steadystate Kalman Gains nIter = 40;
for i=1:nIter Swk = SigW; [~,Sxk1k,~,Sxkk,Sykk1,Lk,Hk,Gk,~] = …
KF231B([],Sxkk1,A,[],C,E,F,Swk,[],[]); Sxkk1 = Sxk1k;
end

Form Kalman filter with 3 outputs
AKF = ALk*C;
BKF = Lk;
CKF = [eye(nX);eye(nX)Hk*C;Gk*C];
DKF = [zeros(nX,nY);Hk;Gk];
SSKF = ss(AKF,BKF,CKF,DKF,TS);
%% Form matrix to extract estimate of y2_{kk}

We need [0 C2]*xhat_{kk} + [0 F2]*what_{kk}. Everything

is scalar dimension, but we can form this matrix properly

so that the example would work on other systems too.
M = [zeros(nY,nX) zeros(nY,nX1) C2 zeros(nY,nW1) F2];
9 of 13
Available: March 8 Due: March 15
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
%% Bode plot of filter

It makes sense that the filter will try to “pass” some

low frequencies, to preserve y2, but will cutoff

highfrequencies to reject y1. The “pass” region should

extend over the region where P2 has modest gain. The Bode

magnitude plot confirms this bodeplot(P2,ārā,P1,ākā,M*SSKF,bOpt) legend(āP2ā,āP1ā,āFilterā);
%% Single Simulation

Create a w sequence consistent with variance assumption wSeq = randn(100,2);
%% Get y1 and y2 (separate simulations) for later comparison y1 = lsim(P1,wSeq(:,1));
y2 = lsim(P2,wSeq(:,2)); y = y1 + y2;
%%

Form the cascade (system output goes directly to Kalman

Filter), and simulate, obtaining the outputs of Kalman

Filter
Est = lsim(SSKF*ss(A,E,C,F,TS),wSeq);
%% Form Estimate of y2

Est matrix is 100by6, so use transpose correctly to do

reconstruction as a matrix multiply
y2Est = (M*Estā)ā;

Plot subplot(1,2,1); plot(0:99,y2,āb+ā,0:99,y2Est,ākoā,0:99,y,ār*ā); legend(āy2 (actual)ā,āy2 (Est)ā,āy (Measured)ā); subplot(1,2,2); plot(0:99,y2,āb+ā,0:99,y2Est,ākoā); legend(āy2 (actual)ā,āy2 (Est)ā);
end code


In the code cell above, we iterated 40 steps to get the \steadystate” gains, but did not verify convergence. Using simple methods, estimate approximately how many steps are actually needed for \convergence.”



Follow the main ideas of the template KFexamplesTemplateWithSignalsButNoControl.m to rerun this example using the timevarying (not steadystate) lter. Compare the estimates achieved by the timevarying lter and the steadystate lter over the rst 20 time steps.


Converting between Kalman lter formulations: In our class notes, we formulated the disturbance/noise as one vector valued noise variable, w_{k} 2 R^{n}w , with two matrices, E 2 R^{n n}w and F 2 R^{n}y ^{n}w where Ew additively a ects the state evolution, and F w additively a ects the
10 of 13
Available: March 8 Due: March 15
measurement. The variance w_{k} was _{W} 2 R^{n}w ^{n}w . By contrast, in the Matlab kalman implementation, there are two separate disturbance/noises, labeled d 2 R^{n}d and v 2 R^{n}y . The state evolution is additively a ected by Gd, and the measurement by Hd + v for matrices G 2 R^{n}x ^{n}d and H 2 R^{n}y ^{n}d . The variance of the combined vector is

E
d_{k}d_{k}^{T}
d_{k}v_{k}^{T}
_{=}
Q N
v_{k}d_{k}^{T}
v_{k}v_{k}^{T}
N^{T} R
Suppose the problem is speci ed in the Matlab format, namely
fn_{d}; G; H; Q; N; Rg
How do you convert this description into our class format, namely
fn_{w}; E; F; _{W} g
so that the problems are equivalent? Hint: Look at the means and variances of

Hd + v
and
_{F w}
Gd
Ew
For the problems to be equivalent, these statistical properties need to be equal.

This problem generalized fact #10 in the Kalman derivation. Suppose D_{k} and R_{k} are known matrices, and a random variable _{k} is de ned _{k} := D_{k}x_{k} + R_{k}w_{k}.

^
_{1} := L( _{k}jy_{0}; y_{1}; : : : ; y_{k}
_{1}). Show that
(a) De ne _{kjk}
^
kjk 1 ^{=} ^{D}k^{x^}kjk 1
^
:= L( _{k}jy_{0}; y_{1}; : : : ; y_{k} _{1}
; y_{k}). Show that
(b) De ne _{kjk}
^{^}_{kjk} = D_{k}x^_{kjk} + R_{k}W_{k}F_{k}^{T} _{k}^{y}_{jk} _{1}
1
^{e}k

In the derivation in class, there was a missing variance calculation, associated with x^_{kjk} in Fact
16. It is easy to derive this. Recall the setup for Fact 16:

Z = x_{k}; P = Y_{k} _{1}; Q = y_{k}
We want to determine _{k}^{x}_{jk}, which is de ned to be _{x}_{k}
_{x^}_{kjk} . In terms of Z; P and Q, this is
x_{k} x^_{kjk} ^{=} Z (ZjP;Q)
The general formula for _{Z} _{(ZjP;Q)} is
Z (ZjP )Z (ZjP );Q _{Q}^{1}
(Q P) Z^{T}
(Z
P);Q
j
j
as derived in Fact #6. Fact #16 has all of these terms (for the speci c choices of Z; P; Q). Task:
Substitute these expressions to derive

_{k}^{x}_{jk} = _{k}^{x}_{jk} _{1} _{k}^{x}_{jk} _{1}C^{T} _{k}^{y}_{jk} _{1}
1
C _{k}^{x}_{jk} _{1}
as claimed in the nal Kalman Filter equations.
11 of 13
Available: March 8 Due: March 15

(nothing to turn in – please read carefully) In the derivation in class, there was no control input in the dynamics – we derived the Kalman Filter under the dynamics
x_{k+1} = A_{k}x_{k} + E_{k}w_{k}
If the dynamics include control, then
x_{k+1} = A_{k}x_{k} + B_{k}u_{k} + E_{k}w_{k}
There are a few situations to consider:
Case 1: the sequence fu_{k}g^{1}_{k=0} is deterministic and known. This occurs if the signal u_{k} is just a prespeci ed forcing function.
Case 2: the value of u_{k} is a linear combination of Y_{k}, say u_{k} = J_{k}Y_{k} for some deterministic matrix J_{k}. Note that the dimension of J_{k} changes with k. In this case, since Y_{k} is a random variable, it follows that u_{k} is a random variable. This situation occurs if there is a feedback controller mapping the measurement y_{k} to u_{k} through a linear, timevarying dynamical system.
Case 3: the value of u_{k} is a linear combination of Y_{k} and a deterministic, known signal. This situation is a combination of the two simpler cases, and is very common (a feedback controller with an external reference input). Once they are understood, handling this case is quite easy.
In order to modify the derivation, it is important to consider all of the slides, starting with some of the batch matrix manipulations, and especially at Fact #8. Regarding Fact #8, the two cases di er as follows:

For Case 1, the expression for x_{k} still is linear in x_{0} and W_{k} _{1}, but also has a linear term with U_{k} _{1}. Since u is deterministic, this only changes the mean of x_{k}, but not any correlations or variances.

For Case 2, since y_{k} is a linear combination of x_{k} and w_{k}, the expression for x_{k} is of the identical form (linear in x_{0} and W_{k} _{1}, but the matrices are di erent, and involve B_{0}; B_{1}; : : : ; B_{k} _{1} and J_{0}; J_{1}; : : : ; J_{k} _{1}.
Because of these simple di erences, the boxed conclusions from Fact 8 remain true:
x_{k};w_{k} ^{= 0;} Y_{k} _{1};w_{k} ^{= 0:}
Since Facts 918 do not involve the evolution equation for x_{k+1}, they are unchanged.
Fact 19 does change, since it involve the evolution equation. For Case 1, the term B_{k}u_{k} is simply added to the update, since it is not a random variable. For Case 2, the evolution is x_{k+1} = A_{k}x_{k} + B_{k}J_{k}Y_{k} + E_{k}w_{k}. The lineariy of the a ne estimator means that
L(x_{k+1}jY_{k}) = L(A_{k}x_{k} + B_{k}J_{k}Y_{k} + E_{k}w_{k}jY_{k})

A_{k}L(x_{k}jY_{k}) + B_{k}J_{k}L(Y_{k}jY_{k}) + E_{k}L(w_{k}jY_{k})

^{A}k^{x^}kjk ^{+} ^{B}k^{J}k^{Y}k ^{+} ^{E}k^{w^}kjk

^{A}k^{x^}kjk ^{+} ^{B}k^{u}k ^{+} ^{E}k^{w^}kjk
where we used the fact that L(Y_{k}jY_{k}) = Y_{k} and u_{k} = J_{k}Y_{k}.
12 of 13
Available: March 8 Due: March 15
So, the change to the boxed expression on Fact 19 is simply the addition of the term B_{k}u_{k}, in both cases.
Fact 20 is unchanged. Fact 21 involves the expression x_{k+1} x^_{k+1jk}, so it must be studied. However the only di erence (for both Case 1 and 2) is that now both terms, x_{k+1} and x^_{k+1jk} include the additional additive term B_{k}u_{k}. By subtraction, these cancel, and the calculation of the variances is una ected.
Hence, for both Case 1 and Case 2 (and Case 3, by a simple argument involving both ideas), the only change to the Kalman lter equations is the inclusion of the b_{k}u_{k} term in the State Prediction equation, namely

x^_{k+1jk} = A_{k}x^_{kjk} + B_{k}u_{k} + E_{k}W_{k}F_{k}^{T} _{k}^{y}_{jk} _{1}
1
e_{k}
13 of 13