Assignment: Kalman Filtering (part 2) Solution

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 batch-estimation: In class (and the notes) we derived the batch-
formulation of Kalman ltering, generating the correct matrices to compute L(xkjy0; : : : ; yk 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(x0jy0; : : : ; yk 1); or L(xkjy0; : : : ; yk 1; yk)




In this problem, we introduce the appropriate matrices (and some recursions to make building the batch matrices easier) to compute



02
x1
3






y
0
1




x0




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






5C


@4


5










A






Remark: You can easily modify the ideas below to get batch solutions to



02
x1
3


2
y1
31




x0






y0


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: xk 2 Rnx ; yk 2 Rny ; wk 2 Rny




Relevant Matrices: Note that I am using blue to distinguish the state-evolution matrix Ak at time k from the caligraphic Ak;j.






Ak;j := Ak
1 Aj 2 Rnx nx
k j 0




Nk :=
Ak;1E0 Ak;2E1
Ak;3E2Ak;k
1Ek 2 Ek 1


2 Rnx knw



1 of 13
Available: March 8 Due: March 15















2




E0




0






0








0
0
3


















0




0






0








0
0












k :=
6
A2;.1E0


E.
1






0.




.


0.
0.
7


R
(k+1)nx


knw
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;1E0


A






k;3E2


k;k 1Ek 2
Ek 1
7












6
A


k;2E1






A
7












4






A






A










5










3






I














2


C1


1;0
3














A2;0






























6




1;0
7












6


C0
7
































A












k :=
A.


R
(k+1)nx


nx
; Rk :=


C2
2;0


R
kny


nx
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








x0
















2
w1
3












2


y1
3
















x2












































6
x1


7












6
w0
7












6


y0
7




























































k :=
.




R
(k+1)nx


k 1
:=
w2


R
knw


k 1
:=


y2


R
kny




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
xk


7












6


7












6


7














6


7












4




5












4




5














4




5


















































2


C1E0






F1






0










0




0
3






6


F0






0






0










0




0
7




Sk :=


C2A.1E0






C2.E1






F.2




.




0.




0.
2
Rkny 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 ni-tions below, I’ve included, correctly, the nonzero dimension.




0 = empty; 0nx 0;
0 = Inx nx ;N0 = empty; 0nx 0;
S0 = empty; 00 0;
R0 = empty; 00 nx ;A0;0 = Inx nx
Recursions: for i 0
































Ai+1;0 = AiAi;0;
i+1 =
i
; Ni+1 = AiNi Ei
;
Ai+1;0
2 of 13
Available: March 8






 






Due: March 15




























i+1
=


Ni+1
x
w
; Ri+1
=
CiAi;0
; Si+1
=
CiNi
Fi






i
0(i+1)n
n






Ri




Si
0iny
nw



For i 0, the quantities

fAi+1;0; i+1; Ni+1; i+1; Ri+1; Si+1g

can be calculated (recursively) from




fAi;0; i; Ni; i; Ri; Sig and fAi; Ei; Ci; Fig












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:




Xk = kx0 + kWk 1; Yk 1 = Rkx0 + SkWk 1




De ne Z := Xk; P := Yk 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 TZ;P






3 of 13
Available: March 8
 


Due: March 15


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


Z = km0;P = Rkm0
and










T
+
T


Z = k 0 k
k W k


T




T


P = Rk 0Rk
+ Sk W Sk


T
+


T


Z;P = k 0Rk
k W Sk
Plugging in everything gives our optimal estimate of all states from x0 to xk, based on all of the

measurements y0 to yk
1,








Rk 0RkT + Sk W SkT 1 (Yk 1










L(XkjYk 1) = k 0RkT + k W SkT
Rkm0) + km0
Simple regrouping
























































L(XkjYk 1) =






T
+




T












T






T


1
Yk 1










k 0Rk
k W Sk


Rk 0Rk
+ Sk W Sk














|




k
k 0






k{z


W






k 0


+ Sk
}
W






k 0






h




k
:=Lkbatch


k batch


k












k
1
i
































+






R
T






T
R R
T








T
R m










+S










S










|














































}
























:=V{zk
























and error variance

Xk (XkjYk 1) =
T
+


k 0 k
k W
( k 0RkT +







T






















k
























T
T
T
1
(
T
+
T
T
k W Sk )(Rk 0Rk
+ Sk W Sk )


k 0Rk
k W Sk )


The diagonal block-entries of (each of dimension nx nx) are most important, as they signify the variance (\spread") in the error of the estimate of each time-instance of the state.




Summarizing: for each k 1, there exist matrices Lbatchk and Vkbatch such that




L(XkjYk 1) = LbatchkYk 1 + Vkbatchm0















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,...,k-1. This needs the state-space matrices



defined on i = [0,1,...,k-1]



for i=0:k-1




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 3-dimensional array, of dimension [nX, nX, N], where N k, and for each nonnegative i, Amat(:,:,i+1) equals Ai (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 Lk
is de ned as the last 2nx rows of Lk
and Vk
as the last 2nx rows of Vk
example with the code




begin code







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



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













end code




then












x^kjkj


1
= L
xk




Yk 1
= L~kYk 1 + V~km0


k
1


x^k 1
1


xk


































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 x4 based on y0; y1; : : : ; y9).




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 batch-code to get the best estimate of Xk from Yk 1. The code displays certain rows of Lbatchk and Vkbatch 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(end-2*nX+1:end-nX,:) VkBatch(end-2*nX+1:end-nX)




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 Yk 1, and using

m0 to obtain the estimate xk 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 con-dition, 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(end-2*nX+1:end-nX,:)



VkBatch(end-2*nX+1:end-nX)



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 Lk; Hk; Gk and the estimate w^kjk as de ned in the slides. This should only require adding one line of code to properly de ne Lk, and one or two lines for w^kjk. If you look carefully at the existing code, you will see that Hk and Gk 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_{k|k-1}



%Sxkk1 = Sigma^x_{k|k-1}



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



%Swk = variance in zero-mean disturbance w_k



%uk = deterministic control input u_k



%yk = measurement y_k



% The output arguments are:



%xk1k = xhat_{k+1|k}



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



%xkk = xhat_{k|k}



%Sxkk = Sigma^x_{k|k}



%Sykk1 = Sigma^y_{k|k-1}



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



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



%Hk (from fact #16), for:



18
%
xhat_{k|k} = x_{k|k-1} + Hk*(yk - Ck*x_{k|k-1})



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



%wkk = what_{k|k}



% 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







Steady-State Behavior: As mentioned in class, if the process state-space matrices (Ak; Ek; Ck; Fk) and the variance of wk (which we now notate as Wk := E(wkwkT )) are constant (ie., not time-varying), then (under very mild technical conditions, which we will not worry about) the gain and variance matrices in the Kalman lter converge to steady-state 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 Li+1 and Li (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 P1 and P2 are linear systems (possibly time-varying), with known state-space models. The input to system P1 is v (producing output y1), and the input to system P2 is d (producing output y2). Both v and d are random sequences, with the following properties (for all k and j 6= k)
E(vk) = 0; E(dk) = 0;




and




E(vkvkT ) = Vk; E(dkdTk ) = Dk; E(vkdTk ) = 0; E(vkvjT ) = 0 E(dkdTj ) = 0; E(vkdTj ) = 0 Let y := y1 + y2. Draw a simple block diagram, and label y1 as \noise" and y2 as \signal."




Explain how we can use the Kalman ltering theory to estimate y2 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 y2 as a meaningful signal, which is additively corrupted by noise, y1, to produce a measurement y. The goal of ltering is to recover the meaningful signal (y2) 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 time-index, but the subscript on y1 and y2 is just referring to two di erent signals (each of which depends on time).




In the commented example below, P1 is a rst-order high-pass lter, and P2 is a rst-order low-pass lter, and the separation is done using the steady-state 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 discrete-time example, set TS=-1. In Matlab, this



% just means an unspecified sampling time, totally within the



% context of pure discrete-time systems.



TS = -1;



9




%% Create high-pass 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 low-pass 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 = [1e-4 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 steady-state 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 = A-Lk*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_{k|k}




We need [0 C2]*xhat_{k|k} + [0 F2]*what_{k|k}. 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



high-frequencies 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 100-by-6, 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 \steady-state" 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 time-varying (not steady-state) lter. Compare the estimates achieved by the time-varying lter and the steady-state lter over the rst 20 time steps.



Converting between Kalman lter formulations: In our class notes, we formulated the dis-turbance/noise as one vector valued noise variable, wk 2 Rnw , with two matrices, E 2 Rn nw and F 2 Rny nw 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 wk was W 2 Rnw nw . By contrast, in the Matlab kalman imple-mentation, there are two separate disturbance/noises, labeled d 2 Rnd and v 2 Rny . The state evolution is additively a ected by Gd, and the measurement by Hd + v for matrices G 2 Rnx nd and H 2 Rny nd . The variance of the combined vector is

E
dkdkT
dkvkT
=
Q N


vkdkT
vkvkT
NT R
Suppose the problem is speci ed in the Matlab format, namely




fnd; G; H; Q; N; Rg




How do you convert this description into our class format, namely




fnw; 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 Dk and Rk are known matrices, and a random variable k is de ned k := Dkxk + Rkwk.



^
1 := L( kjy0; y1; : : : ; yk
1). Show that


(a) De ne kjk






^






kjk 1 = Dkx^kjk 1


^
:= L( kjy0; y1; : : : ; yk 1
; yk). Show that


(b) De ne kjk




^kjk = Dkx^kjk + RkWkFkT kyjk 1
1


ek
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 = xk; P = Yk 1; Q = yk




We want to determine kxjk, which is de ned to be xk
x^kjk . In terms of Z; P and Q, this is
xk x^kjk = Z (ZjP;Q)




The general formula for Z (ZjP;Q) is






Z (ZjP )Z (ZjP );Q Q1
(Q P) ZT
(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




kxjk = kxjk 1 kxjk 1CT kyjk 1
1
C kxjk 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



xk+1 = Akxk + Ekwk




If the dynamics include control, then




xk+1 = Akxk + Bkuk + Ekwk




There are a few situations to consider:




Case 1: the sequence fukg1k=0 is deterministic and known. This occurs if the signal uk is just a prespeci ed forcing function.




Case 2: the value of uk is a linear combination of Yk, say uk = JkYk for some deterministic matrix Jk. Note that the dimension of Jk changes with k. In this case, since Yk is a random variable, it follows that uk is a random variable. This situation occurs if there is a feedback controller mapping the measurement yk to uk through a linear, time-varying dynamical system.




Case 3: the value of uk is a linear combination of Yk 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 xk still is linear in x0 and Wk 1, but also has a linear term with Uk 1. Since u is deterministic, this only changes the mean of xk, but not any correlations or variances.



For Case 2, since yk is a linear combination of xk and wk, the expression for xk is of the identi-cal form (linear in x0 and Wk 1, but the matrices are di erent, and involve B0; B1; : : : ; Bk 1 and J0; J1; : : : ; Jk 1.



Because of these simple di erences, the boxed conclusions from Fact 8 remain true:




xk;wk = 0; Yk 1;wk = 0:

Since Facts 9-18 do not involve the evolution equation for xk+1, they are unchanged.




Fact 19 does change, since it involve the evolution equation. For Case 1, the term Bkuk is simply added to the update, since it is not a random variable. For Case 2, the evolution is xk+1 = Akxk + BkJkYk + Ekwk. The lineariy of the a ne estimator means that




L(xk+1jYk) = L(Akxk + BkJkYk + EkwkjYk)

AkL(xkjYk) + BkJkL(YkjYk) + EkL(wkjYk)
Akx^kjk + BkJkYk + Ekw^kjk
Akx^kjk + Bkuk + Ekw^kjk
where we used the fact that L(YkjYk) = Yk and uk = JkYk.







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 Bkuk, in both cases.







Fact 20 is unchanged. Fact 21 involves the expression xk+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, xk+1 and x^k+1jk include the additional additive term Bkuk. 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 bkuk term in the State Prediction equation, namely




x^k+1jk = Akx^kjk + Bkuk + EkWkFkT kyjk 1
1
ek
















































































































































13 of 13