Solved–Assignment: –Kalman Filtering (part 3)– Solution

Description

Description

Related products

GTLegend: A Role-Playing Game SOlution

Homework 1 Solution

Lab 7: Multithreaded Numerical Integration Solution

Mario Level Generation Solution

Project 1A: Transformation Matrices Solution

Name: Solved--Assignment: --Kalman Filtering (part 3)-- Solution
SKU: 26689
Price: 35.00 USD
Availability: InStock

5/5 – (2 votes)

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 ^Lk^Ck^{)^x}kjk 1 ⁺ ^Lk^yk
^{x^}kjk 1		=	^{Ix^}kjk 1
	^{x^}kjk	=	(I H_kC_k)^x_kjk ₁ + H_ky_k
	^{w^}kjk	=	^Gk^Ck^{x^}kjk 1 ⁺ ^Gk^yk

where formula for L_k; H_k; G_k are given in terms of the iterated variances, all derived tjhough-out Part3 of the slides.

(b) Treating _k := x^_kjk ₁ as the state of the Kalman lter, the equations can be rewritten as

k+1

^{x^}kjk 1

^{x^}kjk

^{w^}kjk

= (A_k L_kC_k) _k + L_ky_k

I _k
(I H_kC_k) _k + H_ky_k
G_kC_{k k} + G_ky_k

Let K denote this system, with y_k as the input, and x^_kjk ₁; x^_kjk and w_kjk as the outputs. Simply verify that the state-space model of K is

(A_k L_kC_k) L_k

K :	6	I	0	7
K :	6	I H_kC_k	H_k	7
	6	G_kC_k	G_k	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 L_k, H_k and G_k), 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 y_k and outputs x^_kjk ₁; x^_kjk and w_kjk

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

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 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

x₁(k + 1) = x₁(k) + x₂(k)

x₂(k + 1) = x₂(k) + x₃(k)

x₃(k + 1) = x₃(k) + w₁(k)

y₁(k) = x₁(k) + w₂(k)

y₂(k) = x₃(k) + w₃(k)

where x₁ is the position, x₂ is the velocity and x₃ is the acceleration. Since w will be modeled with zero mean, the model dictates that the acceleration x₃ is \roughly” constant, being \driven” by a zero-mean, random signal w₁. Measurements y₁ and y₂ 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 ₌² 0₀:3		1	₀ 3
4		0	0	5
	0	0	1

Find the steady-state Kalman lter gain L, and the steady-state error-variance, lim_k!1 ^x_kjk. 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 (x₂) 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 x₀ and w_k (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(x₀) = 0_{3 1}; E(w_k) = 0_{3 1}; E(w_kw_k^T ) = _W ; E(w_kw_j^T ) = 0_{3 3} for k 6= j

1. Draw a histogram of the estimation error in x₂ (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	=	Ax_k + Bu_k
y_k	=	Cx_k + Du_k

with (A) < 1. Recall for any square matrix M with eigenvalues ₁; ₂; : : : ; _n, the spectral radius is de ned (M) := max₁ _{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 e^{j k}, de ne the frequency-response function

G( ) := D + C(e^j I A) ¹B

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

is a sequence u₀; u₁; : : : of random variables, such that for all k; j, with k 6= j

E(u_k) = 0_n_u ; E(u_ku^T_k ) = I_n_u ; E(u_ku^T_j ) = 0_n_u

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

The behavior of the mean of y_k 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

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

₁ ^Z 2

2 ₀

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

kHk₂	:=			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 k₂) is to be computed

5 of 10

Available: March 20 Due: April 5

begin code

sqrt(trace(Int))

norm(H,2)

end code

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

lim E(y_ky_k^T )

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;

% 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.

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

NsampleSpace = 2000;

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

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

% 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;

% 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

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 lim_k!1 E(y_ky_k^T ) 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(u_k) = 0_n_u ; E(u_ku^T_k ) = _U ; E(u_ku^T_j ) = 0_n_u

where _U is a given, positive semide nite matrix. We previously computed for _U = I_n_u , 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
				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 x₃) 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

and x₃(k + 1) = x₃(k).

7 of 10

Available: March 20 Due: April 5

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

We need the equations in the form

x_k+1 = f(x_k) + Ew_k; y_k = h(x_k) + F w_k

as well as the derivative matrices _dx^df and ^dh_dx which are used in the EKF. These have function declaration lines

begin code

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

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

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 ^x_kjk. 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

1. Are initial conditions for the simulation being set randomly, and \consistent” with the probabalistic model for x₀?

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

1. Look at the parameters ( ₀; ; g) and the typical altitudes and velocities at the beginning of the simulation. Why does the estimator not \’learn” much about x₃ 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 =

C_G

D_G

; P =

C_P

D_P

; K =

C_K

D_K

A_G

B_G

A_P

B_P

A_K

B_K

De ne P_e, with state-space model as

A_P B_P

Consider the interconnection

I^P

₀P ⁵

^y –

z _–

^{x^}P _–

e _–

–

^Pe

f₊

–

^xP

(a) Verify that a realization of the interconnection, with input q, output e and states [x_G; x_P ; x_K ] is

2	^BP ^CG	^AP	0	^BP ^DG	3
6	^AG	0	0	^BG	7
6	B_KD_P C_G	B_KC_P	A_K	B_KD_P D_G	7
6	MD_KD_P C_G	I MD_KC_P	MC_K	MD_KD_P D_G	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: P_e 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 x_P and x_G, but the matrix M selects o only the estimate of x_P , which can be compared to the true value (at least in a simulation model, where the true value x_P is known). Nothing to turn in here – make sure that explanation is clear.

10 of 10

_k!1 ^E(yk^{) = 0;}

E(y_ky_k ) = ₂ ^Z

G( )G ( )d