Download Homework Assignments

Fundamentals of Kalman Filtering
Homework Assignments
Chapter 1
1. If A has eigenvalue  then A2 has eigenvalue 2. Is the converse true? That is, if
matrix A’s eigenvalues are the square of matrix B’s eigenvalues, does that mean that
A = B2?
2. Prove that Tr(AB) = Tr(BA).
3. x  Ax  Bu
 1 1 
A

 0  1
1 
B 
0 
Find eAt. Find x(t) when x(0) = [1 1] T, and u(t) = 1 for t  [0, 1], u(t) = 0 for all
other t.
4. Consider the nonlinear system x’’’ + 10x’’ + x’ + 2x2 = –u.
Linearize the equation around u = 0.
Use Matlab to numerically integrate the system using both the linearized and
nonlinear equations for u = 0.01 sin t, and u = 10 sin t. Observe the degradation in the
linearized model as u gets farther from the linearization point.
5. The dynamics of a motor winding can be modeled with the following equations:
V  Ri  Li  K
T  Ki  J  B
V is the input voltage, R is the winding resistance, L is the winding inductance, i is the
winding current, K is the motor constant,  is the rotor position, T is the output
torque, J is the moment of inertia, and B is the damping coefficient.
a. Write a state-space description of the above system, where V is the input, and  is
the output.
b. Is the system linear? Is the system causal? Is the system time-invariant?
c. Find the observability matrix for the system. Is the system observable?
d. Find the controllability matrix for the system. Is the system controllable?
Chapter 2
1. Use Matlab’s “rand” function to generate uniformly distributed random numbers
between 0 and 1. What is theoretical mean and standard deviation? What is the
experimental mean and deviation when the number of random numbers is 10, 100,
and 1000?
2. (Gelb) Suppose x and y are independent normal random variables, both with a
standard deviation of . Show that the variable
z  x2  y2
has the pdf
z
 2 exp(  z 2 / 2 2 ) z  0
pdf ( z )   

0
z0
This is called a Rayleigh density. Show that the mean and variance of z are
  / 2 and (2   / 2) 2
3. In a four-hand card game of Hearts with four cards in the kitty, what is the probability
that the Queen of Spades will be in the kitty two hands in a row?
4. (Brown) A stationary Gaussian process x(t) has power spectrum
2
x ( )  4
 1
Find E(x) and E(x2).
Chapter 3
1. Write a recursive least squares routine to fit a quadratic curve and a cubic curve to the
following equally spaced data:
[1 27 33 45 12 16 83 67 54 39 23 6 14 15 19 31 37 44 56 60].
Generate a plot comparing the raw data with the least squares quadratic fit, and a plot
comparing the raw data with the least squares cubic fit. Compare the RMS error of
the quadratic fit and the cubic fit.
2. Repeat the above problem if the variance of the measurement increases linearly from
one at the first measurement to 10 at the last measurement. This is a weighted least
squares problem.
3. Suppose a signal and noise are independent and have autocorrelation functions
4 t
t
and Rn (t )  3 . Find the noncausal Wiener filter to estimate the
Rx (t )  4e
signal. Find the causal Wiener filter to estimate the signal.
Chapter 4
1. We are given the system x’ = Ax + Lw, where A = [0 2;  1 3] , L = [0; 1] , and
w is zero-mean white noise with an autocorrelation equal to 1.
a. Find a closed-form expression for m(t) (the mean of the state) as a function of
m(0).
b. Give the differential equation that describes the dynamics of P(t) (the covariance
of the state).
c. Suppose the continuous time system is sampled with a sample period of T. Give a
closed-form expression for mk (the mean of the state as a function of sample
number) as a function of T and m(0).
d. For the sampled system, the covariance of the state obeys the equation
Pk =  Pk 1  + Q. Matrices  and Q are constant since the system is LTI. Give a
closed-form expression for the Q matrix as a function of T.
Chapter 5
1. Consider the following system.
0
 0.8 0.1
x   0
 0.5 0.1 x  w
 0
0
0.1
z  1 0 0x  n
Discretize the state space equation with a sample time of 0.1. Design, code, and
simulate a discrete time Kalman filter for the following discrete time process noise
and measurement noise covariances:
 Q = I and R = 0.25.
 Q = 10 I and R = 0.25.
 Q = I and R = 2.5.
 Q = 10 I and R = 2.5.
a. Which of the four cases above results in the biggest difference between the
estimation error and the measurement error? Why?
b. Which of the four cases results in the largest Kalman gain? Why?
c. How does the Kalman gain compare between the first and last cases? Why?
Chapter 6
1. What is the advantage of the sequential Kalman filter over the standard Kalman
filter? When is it not appropriate to use the sequential Kalman filter?
2. Under what conditions is it computationally cheaper to use the discrete time
information filter instead of the standard Kalman filter?
3. What is the advantage of the square root filter over the standard Kalman filter? What
is the disadvantage of the square root filter?
Chapter 7
1. Find the steady state estimation error covariance and Kalman gain of the Kalman
filter for the following system.
x k 1  x k  wk
z k  x k  nk
wk ~ N (0, Q), Q  2
nk ~ N (0, R), R  2
E (nk w j 1 )  M k  j , M  1
Chapter 8
1. Consider a continuous time second order system. The measurement consists of the
first state (corrupted by noise). The process noise is scalar. The parameters are
L = [ 0 ; 1],  = 1,  = 1/2, Rc = 1, and Qc = 1.
a. Find the three coupled differential equations for the three elements of P. These
equations describe the dynamics of the estimation error covariance of the
continuous time Kalman filter.
b. Is (F, L) controllable? Is (F, H) observable? What does this tell you about the
existence and stability of the solution of the algebraic Ricatti equation that is
associated with the steady state Kalman filter for this problem?
c. Solve for the steady state value of the Ricatti equation. Hint: Most of the solution
process can be done by hand, but eventually you obtain a quartic equation of one
variable that must be solved. This is difficult to solve by hand, but easy to solve
by plotting the equation as a function of the variable and picking the solution off
of the plot.
d. Verify that are the closed loop eigenvalues of the estimator are in the left half
plane.
e. Solve for the steady state value of the Ricatti equation using Matlab’s CARE
function. Compare with your hand calculation (to verify that Matlab does not
have any bugs).
f. Repeat steps (a) and (c) for the information matrix. Verify that the steady state
information matrix is the inverse of the steady state covariance matrix.
g. Write a short Matlab program that computes the steady state algebraic Ricatti
equation solution using the transition matrix approach. Verify that the solution
matches your previous answer.
Chapter 9
1. Consider a ship tracking problem. The measurements consists of the range to two
transponders, the velocity, and the heading. So the system is given as follows.
0
0
x  
0

0
0
1
0
0
0  1/
0
0
0 
0 
 0


1 
0
0 
x
u  w, w ~ N (0, diag (4,4,0.25,0.25))
1 / 
0 
0 



 1/ 
 0 1/ 
 ( x1  rn1 ) 2  ( x 2 re1 ) 2 

2
2
( x1  rn 2 )  ( x 2 re 2 ) 
  n, n ~ N (0, diag (100,100,4,0.001))
z
( x32  x 42 )1 / 2


 
x4
 sin 
 2
2 1/ 2 

 ( x3  x 4 )  

x1 = north position, x2 = east position, x3 = north velocity, x4 = east velocity. The two
transponders are located at (north, east) positions (rn1, re1) and (rn2, re2). The gain of the
controller is given by  = 30. Suppose measurements are collected every T = 2 seconds.
Design a hybrid extended Kalman filter (using the small T approximation for Qk) for the
system and simulate it in Matlab under the following conditions.
T
x0  xˆ 0  0 25000 10 0 , P0  diag (100,100,4,4)
u1  20, u 2  0
(rn1 , re1 )  (0,0) (rn 2 , re 2 )  (10 5 ,0)
Plot the four state estimation errors as a function of time, and plot the estimation error
standard deviations as a function of time.
Chapter 10
1. (Gelb) We are given the following system:
x  wk
z  xn
wk ~ N (0, Q), Q  2
nk ~ N (0, R), R  2
Find the steady state covariance for the forward filter and the backward filter. Find
the steady state covariance for the fixed interval smoother. Use the steady state
covariances to find a time invariant smoother.
Chapter 11
1. Suppose we have a system described by the following equations:
x k  x k 1  1
z k  x k  nk
nk ~ N (0,1)
Design a Kalman filter using the following assumed system model:
x k  x k 1  wk
z k  x k  nk
wk ~ N (0, Q)
nk ~ N (0,1)
Run the filter with Q = 0, 0.1, 1, and 10. Make a table comparing the variance of the
state estimate and the mean of the innovations for these four cases. What relationship
do you see between the estimation accuracy and the mean of the innovations? How
does the variance of the innovations compare with the theoretically expected
variance? Now run the Kalman filter using the correct model. Now how does the
variance of the innovations compare with the theoretically expected variance?
Chapter 12
1. Use the radar tracking example from class to design a steady state H-infinity state
estimator use the transfer function approach. What are the estimator gains? How does
the performance compare between the time varying and steady state H-infinity
estimators?
2. Consider the following system, where w and v are noise and m is the signal available
for measurement. We desire to estimate x. Use the variable  to denote the
performance index.
x  x  w  u
m  2x  v
(a) Find the differential Ricatti equation associated with this problem.
(b) Find the solution of the steady-state Ricatti equation. The solution should be a
function of . What is the solution when  = 2?
(c) What is the minimum value of  in order for a steady state solution to exist?
(d) Simulate the time-varying H-infinity estimator that you derived above. Use u = 1,
 = 2 and tf = 10. For the noise terms w and v, use zero-mean and unity-variance
normally distributed random numbers (Matlab’s “randn” function). Plot the
Ricatti equation solution as a function of time. Note that the solution approaches a
constant value. Does this value match the steady state solution for  = 2 as
determined from part (b) above? Rerun your simulation with the steady-state
estimator gain. Do you notice any difference in estimation performance relative to
your previous simulation?