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
x(k + 1) = Fx(k) + v(k) where |
(1) |
F , (2)
v(k) is the process noise, and the process noise covariance is given by
Q (3)
The radar measures the distance, given by | |
z(k) = Hx(k) + w(k) |
(4) |
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:
- The true distance x(k) between the vehicles
- The measured distance z(k)
Plot the following on separate axes:
- Velocity ˙x(k)
- 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)
- Three point initialization (of position, velocity and accelration)
- 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)
- Position (true vs. KF)
- Velocity (true vs. KF)
- Position variance
- Velocity variance
- NIS (along with its performance limits on the same axis)
- NEES (along with its performance limits on the same axis)
Question 4 (Kalman filter vs. RLS filter)
- Develop an RLS filter to estimate x(k).
- Use the simulated data above to compare the RLS filter against KF
- 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
- A filter consisting of 2-states (using the WNA model)
- 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.
- Re-write the state-space model (1)-(4) such that the state x(k) represents the true state of vehicle B
- 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.
- 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
- Assuming the same radar installed in A, develop a new state space model by which A can estimate the state of the vehicle B
- 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)
- 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
- x(k|k) vs x(k|n)
- P(k|k) vs P(k|n) for different types of initializations (three point vs. random)