Assignment: Kalman Filtering (part 3) Solution

Problem set due Thursday, April 5, 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.













1. (a) Verify that the equations for the signal recursion in the estimator (see Part3 slides) can be rewritten as

x^k+1jk
=
(Ak LkCk)^xkjk 1 + Lkyk
x^kjk 1
=
Ix^kjk 1


x^kjk
=
(I HkCk)^xkjk 1 + Hkyk




w^kjk
=
GkCkx^kjk 1 + Gkyk


where formula for Lk; Hk; Gk are given in terms of the iterated variances, all derived tjhough-out Part3 of the slides.




(b) Treating k := x^kjk 1 as the state of the Kalman lter, the equations can be rewritten as




k+1

x^kjk 1

x^kjk

w^kjk




= (Ak LkCk) k + Lkyk

I k
(I HkCk) k + Hkyk
GkCk k + Gkyk



Let K denote this system, with yk as the input, and x^kjk 1; x^kjk and wkjk as the outputs. Simply verify that the state-space model of K is




3
(Ak LkCk) Lk




K :
6
I
0
7
I HkCk
Hk


6
GkCk
Gk
7


4




5
(c) For the steady-state Kalman Filter (problem data A; E; C; F; W is time-invariant, and the lter uses the constant, converged values for Lk, Hk and Gk), write formSteadyStateKF.m which




implements the iteration to converge to constant values,




Implements the state-equations above in part 1b, yielding a system K with input yk and outputs x^kjk 1; x^kjk and wkjk




The function declaration line is shown below along with a signi cant amount of partial code to get you started. The partially completed m- le is included in the .zip le, and you can work from that as a starting point.









1 of 10
Available: March 20 Due: April 5
















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




27




28




29




30




31




32




33




34




35




36




37




38




39




40




41




42




43




44




45







begin code







function [K,L,H,G,nIter,estErrVar] = formSteadyStateKF(A,E,C,F,W,TS)




TS is specified sample time. Use -1 to indicate a discrete-time



system with unspecified sample time.



Get dimensions



nX = size(A,1);




nY = size(C,1);




nW = size(W,1);




Note: Your code should check for consistency of E, F, and C,



but not necessary in this assignment. If you have time, please



use good programming practices.



Initialize value for Sxkk1. Convergence will occur for



any positive-definite initialization.



Sxkk1 = eye(nX);




Initialize ‘‘previous’’ values for L, H, G prevL = zeros(nX,nY);



prevH = zeros(nX,nY); prevG = zeros(nW,nY); iterConverged = false; nIter = 0;




while ~iterConverged nIter = nIter + 1; [~,Sxk1k,~,Sxkk,~,Lk,Hk,Gk,~] = ...




KF231B([],Sxkk1,A,[],C,E,F,W,[],[]);




if norm(Lk-prevL)<1e-8 && norm(Hk-prevH)<1e-8 && ...




norm(Gk-prevG)<1e-8 && norm(Sxk1k-Sxkk1)<1e-8 iterConverged = true;




=



H = G =




K = ss( , ,[ ], [ ],TS);




K.OutputGroup.xkk1 = 1:nX;




K.OutputGroup.xkk = nX+1:2*nX;




K.OutputGroup.wkk = 2*nX+1:2*nX+nW;




The steady-state variance of {hat x}_{k|k} - x_k is Sxkk.



This is a useful output to understand. Using the



Chebychev inequality, you can (probabilistically)



bound the potential error in the state-estimate. estErrVar = Sxkk;



else




Sxkk1 =




prevL =




prevH =




prevG =




end




end




end code









2 of 10
Available: March 20 Due: April 5










Note: This o ers similar functionality to the kalman command in Matlab, but adheres to the notation we used in the slides, and provides several interesting outputs of the estimator. As you move forward with your work and education, you shold be able to \translate" between various manners in which the Kalman lter is written down, and/or programmed.




Since we used the OutputGroups feature of the Matlab Control toolbox (lines 31-33), it is very easy to select o the outputs of K which are most important to you. For example, if you only want (as output) the estimate x^kjk, simply use
begin code







1 K_xkk = K(‘xkk’,:);




end code







There is nothing to turn in here, but I want you to understand how OutputGroups work.




They (along with InputGroups) can be useful when dealing with large, linear systems.







Consider a 1-d problem involving position and velocity estimation from noisy position and accel-eration measurements. The discrete-time model abstraction is



x1(k + 1) = x1(k) + x2(k)

x2(k + 1) = x2(k) + x3(k)

x3(k + 1) = x3(k) + w1(k)

y1(k) = x1(k) + w2(k)

y2(k) = x3(k) + w3(k)




where x1 is the position, x2 is the velocity and x3 is the acceleration. Since w will be modeled with zero mean, the model dictates that the acceleration x3 is \roughly" constant, being \driven" by a zero-mean, random signal w1. Measurements y1 and y2 are noisy measurements of the position and acceleration, respectively. Set = 0:1.




(a) Assume that the variance of the zero-mean sequence w(k) is constant, namely

W =2 00:3
1
0 3
4


0
0
5
0
0
1
Find the steady-state Kalman lter gain L, and the steady-state error-variance, limk!1 xkjk. Use the code in formSteadyStateKF, and remember that your results are approximate (we stop when a numerical convergence criteria is met)




Given the steady-state error-variance computed above, show that the probability that error in the the velocity-estimate (x2) is greater than 0:73 is less than 0:25. Use the Chebychev inequality.



In the .zip le, there are 3 template les for simulation of a time-varying Kalman lter with a time-varying model. They are named



KFexamplesTemplateWithoutSignals.m




KFexamplesTemplateWithSignalsWithoutControl.m KFexamplesTemplateWithSignals.m




3 of 10
Available: March 20 Due: April 5










These require speci cation of the time-varying martrices (A; E; C; F; W ) as well as other variables, depending on the particular le. For this problem, please study these les carefully, and understand how to use them.




Do 600 simulations (using KFexamplesTemplateWithSignalsWithoutControl.m), each of duration 200 steps. Interpret the 600 simulations as coming from a nite sample-space with 600 outcomes, where the random variables x0 and wk (here I am using the subscript to mean time whereas in the state equations for this problem, the subscript meant index, and the time was expressed with a (k) notation) satisfy



E(x0) = 03 1; E(wk) = 03 1; E(wkwkT ) = W ; E(wkwjT ) = 03 3 for k 6= j




Draw a histogram of the estimation error in x2 (here the subscript 2 means the second component of x,. ie., velocity) at the end of the simulation (time-step 200), for all 600 outcomes. Is the nal error greater than 0:73 in less than 25% of the outcomes?



Consider a time-invariant, discrete-time, linear system



xk+1
=
Axk + Buk
yk
=
Cxk + Duk



with (A) < 1. Recall for any square matrix M with eigenvalues 1; 2; : : : ; n, the spectral radius is de ned (M) := max1 i n j ij. The condition (A) < 1 means that the system is asympotically stable (with u 0, all solutions x decay to 0). For 2 [0; 2 ], which is a discrete-frequency, associated with the sinusoid ej k, de ne the frequency-response function




G( ) := D + C(ej I A) 1B




You have already learned that G( ) give information about the steady-state response of the system to (complex) sinusoids of the form uk = uej k. Here we make connections to the average behavior of the system to random inputs. Important Theorem: Suppose the input




is a sequence u0; u1; : : : of random variables, such that for all k; j, with k 6= j



E(uk) = 0nu ; E(ukuTk ) = Inu ; E(ukuTj ) = 0nu




(which we refer to as \unit-intensity, discrete-time, white noise"). Then, regardless of initial condition

k!1 E(yk) = 0;
k!1
E(ykyk ) = 2 Z
0
2


lim
lim
T
1




G( )G ( )d

















The behavior of the mean of yk is easy to understand. We will verify the variance formula (approx-imately) using numerical simulation and numerical integration. It is proven using manipulations of the state equations, and invoking Parseval’s relation, but will not be derived here.




The command H = drss(nX,nY,nU) creates a random discrete-time system, with speci ed number of states, outputs and inputs. The command works such that (A) 1 (but not necessarily (A) < 1), so we may need to scale A by small amount to guarantee stability. For example, the code below creates a 4-state, 3-output, 2-input stable system









4 of 10
Available: March 20 Due: April 5



















1




2




3




4




5







begin code







nX = 4;




nY = 3;




nU = 2;




H = drss(nX,nY,nU);




if max(abs(eig(H.A)))0.999; H.A = 0.99*H.A; end end code







Experiment with this code, perform some step-responses, and use the isstable command to verify that the resulting systems are stable (ie., (A) < 1).



Given such a stable system, we can perform the integration
1 Z 2



2 0

G( )G ( )d



numerically using integral and the command freqresp.

begin code







% Create function_handle which returns integrand (matrix-valued)



% as a function of OMEGA. This uses the command FREQRESP



% which calculates the frequency-response at a single, given frequency.



f = @(Omega) freqresp(H,Omega)*freqresp(H,Omega)’;



%



% Call INTEGRAL, specifying limits of integration as 0 to 2*PI. Make



% sure the command INTEGRAL knows that the integrand is not



% a scalar, but is array-valued (in our case, nY-by-nY)



Int = 1/(2*pi) * integral(f,0,2*pi,’ArrayValued’,true’)



%



% Note that although the integrand is a complex, hermitian



% matrix, the overall integral is purely real. This can be proven



% but we will not focus on that here. Some small numerical



% values for imaginary parts creep in due to roundoff. Hence,



% take the real-part of the computed answer to clean up the result.



Int = real(Int)



end code







Experiment with this code on a few stable systems H.




The command norm, as applied to systems, computes the frequency-domain integral (using state-space calculations, rather than numerical integration), but reduces it to a scalar using the \trace" operation. For stable linear systems, de ne




Tr


2 Z


2
1










kHk2
:=
0
2
G( )G ( )d












1
















Recall that Tr is the trace of a square matrix, which is just the sum of the diagonal entries. Since that is a linear operation, it can also be moved into the integrand without changing the value. Verify that the computed integral above (variable Int), is related (through these expressions) to the results from norm, The use of the second argument, the number 2, signi es the the norm command that this speci c norm (k k2) is to be computed



5 of 10
Available: March 20 Due: April 5



















1




2







begin code







sqrt(trace(Int))




norm(H,2)




end code






(d) Given such a stable system, we can approximate




lim E(ykykT )

k!1




for the white-noise input u using randn and lsim (for simulations). We saw in the rst Kalman Filter homework how to interpret the output of randn as generating a nite (but large, if we like) sample-space, with nite-duration (but as long duration as we like) sequences that have approximately the variances and correlations we want.




The code below carries out this numerical experiment.




begin code







% Decide on the duration (in time) of each input/output sequences.



% The formula has k - INF, but we can only simulate for a finite



% duration. In general, choose a duration "long" compared to the



% time-constants of the system. Here, we simply pick a duration



% of 200 steps.



Nsteps = 200;



7




% So each instance (associated with a point in the sample space)



% of the u-sequence is an nU-by-200 array. The convention in



% Matlab’s simulation codes (lsim, Simulink, etc) is to



% represent this as a 200-by-nU array.



12




% Decide on the dimension of the sample-space. Take 2000 outcomes,



NsampleSpace = 2000;



15




% Hence, we need 2000, 200-by-nU arrays, as generated by RANDN.



% Create a 200-by-nU-by-2000 array with randn.



U = randn(Nsteps,nU,NsampleSpace);



19




% Initialize a nY-by-nY matrix to hold the expectation



% of the final value (of each simulation) of y*y^T.



E_y_yT = zeros(nY,nY);



23




% Our construction (from RANDN) had all outcomes equally likely,



% with probability equal to 1/NsampleSpace. So, create a



% constant value for each individual outcome’s probability



p = 1/NsampleSpace;



28




% Use a FOR loop to compute the response for all the



% sample-space outcomes



for i=1:NsampleSpace



% Use LSIM to compute each response.



6 of 10
Available: March 20 Due: April 5













33




34




35




36




37




38




39




40




41




42




43




44




45




46




47













Recall that U(:,:,i) is the 200-by-nU array (for input



sequence) associated with the i’th sample point.



Y = lsim(H,U(:,:,i));




By convention (LSIM), the matrix Y will be Nsteps-by-nY.



The last value (our approximation to k-INF) is simply the



last row of Y.



y_end = Y(end,:)’;




E_y_yT = E_y_yT + p*(y_end*y_end’);




end




Compare to limiting expected value to the integral E_y_yT



Int




end code







They are not exactly the same, but if we take longer simulations and a larger sample space, our calculation will be closer to the limiting answer. Since we are already taking a relatively long simulation, the convergence is mostly improved by taking a larger sample space. Recall that RANDN generates input sequences that only approximate the white-noise process, but larger sample-spaces will make this approximation better.



The Matlab command covar makes this computation of limk!1 E(ykykT ) directly (with state-space calculations, not esimated with a nite number of nite-duration simulations) under a slightly more general assumption, namely that for all k; j, with k 6= j
E(uk) = 0nu ; E(ukuTk ) = U ; E(ukuTj ) = 0nu




where U is a given, positive semide nite matrix. We previously computed for U = Inu , so use that for comparison in this numerical example

begin code







covar(H,eye(nU))



2
Int
%
exactly matches output of COVAR
3
E_y_yT
%
approximately matches output of COVAR








end code















The model below is 1st-order Euler approximation for a mass falling through the atmosphere. The drag term accounts for variation in density (as a function of altitude) and damping forces that scale with the square of the velocity. There is also an unknown constant scale factor (modeled by x3) called the \ballistic coe cient", which depend on the exact shape and aerodynamic characteristics of the mass, and is generally not known.



The equations are

x1(k + 1) = x1(k) + x2(k)
x2(k + 1) = x2(k) + 2 0


x (k)2
g3 + w1(k)






x1(k)






e




x (k)2




4




5


3


and x3(k + 1) = x3(k).



7 of 10
Available: March 20 Due: April 5










The states are as follows: x1 is altitude; x2 is velocity; x3 is the (constant, but unknown) ballistic coe cient. The equation for x1 represents that velocity is the derivative of position. The equation for x2 represents Newton’s law, and includes the e ect of random force, represented by w1. The equation for x3 indicates that x3 does not change (as it represents an unknown parameter). There is one measurement, a noisy measurement of the altitude, modeled as y1(k) = x1(k) + w2(k). We want to implement an Extended Kalman Filter (EKF) to estimate all 3 states.




We need the equations in the form




xk+1 = f(xk) + Ewk; yk = h(xk) + F wk




as well as the derivative matrices dxdf and dhdx which are used in the EKF. These have function declaration lines




begin code







function xp1 = FallingObjectDynEq(x,delta,rho0,gamma,g) % implements f(x)



2
function y = FallingObjectOutEq(x,delta,rho0,gamma,g)
% implements h(x)



function A = FallingObjectDynEqJac(x,delta,rho0,gamma,g) % implements df/dx



function C = FallingObjectOutEqJac(x,delta,rho0,gamma,g) % implements dh/dx end code






These are supplied, and correct, and match exactly with equations given above. The Extended Kalman Filter is implemented, with function declaration line







1




2




begin code







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




EKF231BToFinish(xkk1,Sxkk1,dfdx,dhdx,E,F,fhandle,hhandle,Swk,yk) end code






Four (4) lines of code in that le are partially started, but you need to complete them to make it work. Complete these lines, and verify that the other lines are correct, by matching the code with the slides in Part4. Save the completed le to a new name, EKF231B.m



Work out the necessary partial derivatives by hand, and verify that the four FallingObject les are all correct



Examine the script ekfIterationScript.m, which simulates the real system and imple-ments the EKF on each iteration. This le is similar in layout the Template les provided earlier. Make sure you can connect the lines of code to the evolution of the real system and the EKF implementation.



Run the script ekfIterationScript.m. It will make six plots of the state-estimation errors, and the actual state trajectories, and some \bounds" on the errors, using the variance matrix of the state estimation error, namely xkjk. Run the script many times to see the general trends.



Examine the script le to see how the initial conditions x(0) are set, and how \large" the process disturbance and measurement noise are. Answer the following questions






8 of 10
Available: March 20 Due: April 5










Are initial conditions for the simulation being set randomly, and \consistent" with the probabalistic model for x0?



Is the disturbance sequence w(k) being set randomly, and \consistent" with the proba-balistic model for w?



Look at the parameters ( 0; ; g) and the typical altitudes and velocities at the beginning of the simulation. Why does the estimator not \’learn" much about x3 during the early portion of the simulation?






(The code you write for this problem will be used on the midterm) Suppose G, P , K are discrete-time, time-invariant linear systems, and M is a real matrix. Let G, P , and K have realizations
G =
CG
DG
; P =
CP
DP
; K =
CK
DK




AG
BG




AP
BP






AK
BK































De ne Pe, with state-space model as

3
AP BP














P
=
4
C
D










Consider the interconnection
e




IP
0P 5










































































y -


K




z -
M
x^P -




e -
q
G
w
-
Pe




















f+
-




















































6










xP





























































































(a) Verify that a realization of the interconnection, with input q, output e and states [xG; xP ; xK ] is

2
BP CG
AP
0
BP DG
3
6
AG
0
0
BG
7
BKDP CG
BKCP
AK
BKDP DG
6
MDKDP CG
I MDKCP
MCK
MDKDP DG
7
4








5















Write a Matlab function, kfBuildIC.m, with function declaration line begin code






function IC = kfBuildIC(G,P,K,M)



end code







The function should make sure dimensions are compatible, and form the interconnection. The input arguments G, P and K, along with the output argument IC are all Matlab ss objects. The agument M should be a numerical matrix. The sample times of G, P and K should all be the same, and the sample time of IC should match that common value.




I will post some test data that you can use to test your implementation. Look for an Announcement at bCourses.



One application of this is as follows: Pe is the linear, time-invariant plant for which we want to do optimal state estimation. The system G is a signal model for the noise w, where



9 of 10
Available: March 20 Due: April 5










the signal q satis es the \whiteness" assumptions in the KF derivation. The steady-state Kalman Filter K generates an estimate of both xP and xG, but the matrix M selects o only the estimate of xP , which can be compared to the true value (at least in a simulation model, where the true value xP is known). Nothing to turn in here - make sure that explanation is clear.





















































































































































































10 of 10