ELEC 8900 Estimation Filtering and Tracking

ELEC 8900 Special Topics: Estimation, Filtering, and Tracking

Project: Implementation of Kalman Filter

Figure 1 shows two automobiles in a car following model where the objective of the follower vehicle A is to estimate the distance to the vehicle B in front based on radar measurements. The vehicle A continuously measures the distance between the B and itself with the help of a font-bumper mounted radar. Based on the estimated distance, the vehicle adjusts its controls to maintain a constant distance.

z(𝑘) = 𝑥(𝑘) + 𝑤(𝑘) ∀ 𝑘 = 1, … , 𝑛

Figure 1: Measured distance using a radar. The true distance at time k is denoted x(k), corresponding measurement and the measurement noise are denoted z(k) and w(k), respectively.

The state-vector x(k) = [x(k), x˙(k), x¨(k)]^T, consisting of (relative) position, velocity and acceleration, is assumed to undergo the following process model

F , (2)

v(k) is the process noise, and the process noise covariance is given by

Q (3)

where

H = [1 0 0] (5)

is the measurement matrix.

Question 1 (Data simulation)

Let us assume that the radar measurements are taken at a sampling rate of 10Hz. Simulate

100 measurement samples, i..e, k = 1,2,,...,100, for the following assumptions

• Initial state is x(0) = [0, 2, 2]^T
• Use ˜q = .001

Plot the following on the same axis:

1. The true distance x(k) between the vehicles
2. The measured distance z(k)

Plot the following on separate axes:

3. Velocity ˙x(k)
4. Acceleration ¨x(k)

Question 2 (Filter initialization)

Implement the folloiwng two approaches to initialize the Kalman filter (then compare their performance in KF implementations for each question)

1. Three point initialization (of position, velocity and accelration)
2. Random initialization (that could be far away from the true value)

Question 3 (The Kalman filter implementation)

Implement a Kalman filter Plot the following quantities against time (see Figure 5.3.2.-1 on page 220 of the textbook for hint)

1. Position (true vs. KF)
2. Velocity (true vs. KF)
3. Position variance
4. Velocity variance
5. NIS (along with its performance limits on the same axis)
6. NEES (along with its performance limits on the same axis)

Question 4 (Kalman filter vs. RLS filter)

1. Develop an RLS filter to estimate x(k).
2. Use the simulated data above to compare the RLS filter against KF
3. Discuss your comparison results

Question 5 (Model mismatch analysis)

Explain how model-mismatch can be spotted in a Kalman filter. Implement the following model mismatched filters to demonstrate your analysis

1. A filter consisting of 2-states (using the WNA model)
2. A filter with incorrect knowledge of the covariance matrices

Question 6 (Modelling relative vs. true vehicle state — optional)

The model described in (1)-(4) models the relative (position, velocity and acceleration) of B w.r.t. A. In order to find the real values, these quantities need to be translated. For example, the relative velocity xˆ˙(k) = 3 km/h needs to be translated (baed on the true velocity of A). The objective of this question is to develop a new model such that the state x(k) contains the true state of the vehicle B based on the same measurement z(k) which is either the relative distance or relative velocity of the vehicle B w.r.t. A.

1. Re-write the state-space model (1)-(4) such that the state x(k) represents the true state of vehicle B
2. Demonstrate a Kalman filter based assuming the parameters used in Question 1 for WNA model

Question 7 (Joint estimation of both vehicles’ states — optional)

In reality, the vehicle A does not know its true position or velocity — all it has is an estimate of these quantities.

1. Assuming that A has a “measurement” of its true velocity (e.g., through the rpm of its wheels) develop a state-space model that it can use to estimate its true state
2. Assuming the same radar installed in A, develop a new state space model by which A can estimate the state of the vehicle B
3. Demonstrate the above through a simulation (i.e., first, generate the data by making assumptions and then employ a Kalman filter to estimate the states)
4. Your solution to the above was likely in the form of a “joint state estimation” model. Can you compare the performance of this approach to the following
   • Relative to an alternate approach (e.g., estimate own state first and use a method similar to earlier ones)
   • Relative to the true state

Question 8 (Kalman smoother — optional)

Implement a Kalman smoother for the WNA problem (i.e., data generate using WNA and

KS implemented for WNA). Plot the following for comparison no the same axis

1. x(k|k) vs x(k|n)
2. P(k|k) vs P(k|n) for different types of initializations (three point vs. random)