2009-01-0162
Improving Time-To-Collision Estimation by IMM Based Kalman Filter Yixin Chen Delphi Corporation
Manohar Das and Devendra Bajpai Oakland University
Copyright © 2009 SAE International
ABSTRACT
INTRODUCTION
In a CAS system, the distance and relative velocity between front and host vehicles are estimated to calculate time-to-collision (TTC). The distance estimates by different methods will certainly include noise which should be removed to ensure the accuracy of TTC calculations. Kalman filter is a good tool to filter such type of noise. Nevertheless, Kalman filter is a model based filter, which means a correct model is important to get the good filtering results. Usually, a vehicle is either moving with a constant velocity (CV) or constant acceleration (CA) maneuvers. This means the distance data between front and host vehicles can be described by either constant velocity or constant acceleration model. In this paper, first, CV and CA models are used to design two Kalman filters and an interacting multiple model (IMM) is used to dynamically combine the outputs from two filters. In detail, IMM technique is used to estimate the mode probabilities for CV and CA based Kalman filters and mix the two filter results based on the mode probabilities. Then the motion equation is used to calculate the TTC based on the distance and velocity estimates obtained from the IMM based filter. The experimental results on both simulated and real estimated distance data are found to be satisfactory and indicate that the proposed algorithm does improve the signal-to-noise ratio of distance data.
In a CAS system, time-to-collision (TTC) is the most important output which needs to be estimated in realtime based on the distance and relative velocity between front and host vehicles. If TTC is below a threshold, the correspondent action should be taken, such as giving driver a sound warning, or automatically applying brake, etc. The distance estimates by vision or radar based CAS system certainly include noise which should be removed to improve the accuracy of TTC calculations. Kalman filter is a good tool to filter such types of noise. Nevertheless, Kalman filter is a model based filter, which means a correct model is important to get the good filtering results. Usually, a vehicle is either moving with a constant velocity (CV) or constant acceleration (CA). This means the distance between front and host vehicles can be described by either constant velocity or constant acceleration model. In this paper, first we explore the use of an interacting multiple model (IMM) technique [1] to estimate the mode probabilities for CV and CA based Kalman filters and mix the two filter results based on the mode probabilities. Then a simple motion equation is used to calculate the TTC based on the distance and velocity estimates obtained from the IMM based filter.
The Engineering Meetings Board has approved this paper for publication. It has successfully completed SAE’s peer review process under the supervision of the session organizer. This process requires a minimum of three (3) reviews by industry experts. All rights reserved. No part of this publication may be reproduced, stored in a retrieval system, or transmitted, in any form or by any means, electronic, mechanical, photocopying, recording, or otherwise, without the prior written permission of SAE. ISSN 0148-7191 Positions and opinions advanced in this paper are those of the author(s) and not necessarily those of SAE. The author is solely responsible for the content of the paper. SAE Customer Service: Tel: 877-606-7323 (inside USA and Canada) Tel: 724-776-4970 (outside USA) Fax: 724-776-0790 Email:
[email protected] *9-2009-01-0162* SAE Web Address: http://www.sae.org Printed in USA
5. Filter gain:
ESTIMATION OF TIME-TO-COLLISION (TTC) In rear-end CAS application, the distance between front and host vehicles is measured in real-time by the different methods such as radar or computer vision. Given the distance x(k ) between two cars in frame k , the instant speed between two vehicles can be approximated by the derivative of x(k ) , and the time-tocollision
W ( k + 1) = P ( k + 1 | k ) H ' S ( k + 1) −1
(8)
6. Measurement residual: ^
~
v ( k + 1) = Z ( k + 1) − Z ( k + 1 | k ) = Z ( k + 1 | k ) (9)
TTC (k ) in frame k is defined as TTC ( k ) =
x(k ) x ( k )
7. Updated state estimate: ^
X ( k + 1 | k + 1)
(1)
^
In reality, there are always distance estimation errors which can affect the accuracy of TTC estimations. Using the filter techniques to de-noise the distance estimations can improve the accuracy of TTC estimations.
= X ( k + 1 | k ) + W ( k + 1)v ( k + 1) 8. Updated covariance:
P ( k + 1 | k + 1)
= P ( k + 1 | k ) − W ( k + 1) S ( k + 1)W ( k + 1) '
KALMAN FILTER
(11)
Kalman filter is well known for its good performance to remove Gaussian noise and can be described by the following state space equations:
⎧ X ( k + 1) = FX ( k ) + Gu( k ) + Γv ( k ) ⎨ ⎩ Z ( k ) = HX ( k ) + w( k )
(2)
where v(k ) and w(k ) are system and measurement noise, respectively.
A correct model is important while using Kalman filter described above. In rear-end CAS application, the two vehicles can travel in different maneuvers such as cruising in a fixed speed or suddenly acceleration, etc. This means that the distance between two vehicles can be described by either constant velocity (CV) model or constant acceleration (CA) model. A multiple model approach should be used to estimate the distance.
CONSTANT VELOCITY MODEL (CV)
Let us denote
Q = E [Γv ( k )v ′( k ) Γ ' ] = Γσ v 2 Γ ' R = E [ w( k ) w( k ) ' ]
(3)
Then the solution of Kalman filter is given by the following steps [1]: 1. Predicted state: ^
(10)
^
X ( k + 1 | k ) = F X ( k | k ) + Gu ( k )
2. State prediction covariance: '
P ( k + 1 | k ) = FP ( k | k ) F + Q
(4)
(5)
3. Predicted measurement: ^ ^
Z (k + 1 | k ) = H X (k + 1 | k )
4. Measurement prediction covariance: '
S ( k + 1) = HP ( k + 1 | k ) H + R( k + 1)
(6)
(7)
When both front and host vehicles are moving at a constant velocity, the distance data between the two vehicles can be modeled by constant velocity model which is shown as below:
⎧⎡ X ( k + 1)⎤ ⎡ X ( k )⎤ ⎡T 2 ⎤ ⎪⎢ • ⎥ ⎢ 2⎥ ⎥ ⎡1 T 0⎤ ⎢ • ⎪⎢ X ( k + 1)⎥ = ⎢0 1 0⎥ ⎢ X ( k )⎥ + ⎢ T ⎥v ( k ) ⎥⎢ ⎪⎢ •• ⎥ ⎢ ⎥ ⎢ ⎥ 0 0 0 ⎥⎦ ⎢ •• ⎥ ⎢ 1 ⎥ ⎢ ⎪⎢ X ( k + 1)⎥ ⎣ ⎦ ⎪⎣ ⎣ X ( k )⎦ ⎣ ⎦ ⎨ ⎡ X ( k )⎤ ⎪ ⎥ ⎢• ⎪ ⎪ Z ( k ) = [1 0 0] ⎢ X ( k )⎥ + w( k ) ⎢ •• ⎥ ⎪ ⎢ X ( k )⎥ ⎪⎩ ⎦ ⎣
(12)
X (k ) denotes distance estimate in frame k and T denotes the time difference between frame k + 1 and frame k . where
μi ( k − 1) = P{M i ( k − 1) | Z k −1}
CONSTANT ACCELERATION MODEL (CA) If either front or host vehicle is moving with a constant acceleration and the other vehicle is moving at a constant velocity or a different acceleration, the distance data between the two vehicles can be modeled by constant acceleration model (CA) which is shown as below:
⎧ ⎡ X ( k + 1)⎤ ⎡ T 2 ⎤ ⎡ X ( k )⎤ ⎡T 2 ⎤ ⎪⎢ • ⎥ ⎢1 T ⎥ ⎢ 2⎥ 2 ⎥⎢ • ⎪ ⎢ X ( k + 1)⎥ = ⎢0 1 T ⎥ ⎢ X ( k )⎥ + ⎢ T ⎥v ( k ) ⎪⎢ •• ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ 1 ⎥ ⎢•• ⎥ ⎢ 1 ⎥ ⎪ ⎢ X ( k + 1)⎥ ⎢0 0 ( ) X k (13) ⎦⎣ ⎦ ⎪⎣ ⎦ ⎣ ⎦ ⎣ ⎨ ⎡ X (k )⎤ ⎪ ⎢• ⎥ ⎪ ⎪ Z (k ) = [1 0 0]⎢ X (k )⎥ + w( k ) ⎢•• ⎥ ⎪ ⎢ X (k )⎥ ⎪⎩ ⎣ ⎦
THE INTERACTING MUULTIPLE MODEL (IMM) ESTIMATOR The CV and CA models can be used to model the distance between front and host vehicles, but we don’t know when a specific model should be used. The interacting multiple model (IMM) estimator [1] is an algorithm which can be used to handle such case. In IMM algorithm, at time k the previous estimates from the multiple models are mixed based on the mixing probabilities to generate different mixed initial conditions for different filters. Then, based on the multiple filter outputs, the likelihood and then model probabilities for each model can be calculated. Lastly, the final IMM based filter outputs are calculated based on the individual filter outputs and the model probabilities.
a. Calculation of the mixing probabilities (the probability that mode M i was in effect at k − 1 given that M j is k −1
):
Δ
μi | j ( k − 1 | k − 1) = P{M i ( k − 1) | M j ( k ), Z 1 = pij μi ( k − 1) cj
c j = P{M j ( k ) | Z k −1} =
r
∑ pij μi (k − 1)
(17)
j = 1,..., r
i =1
b. Mixing initial conditions for the filter matched to M j (k ) :
xˆ 0 j ( k − 1 | k − 1) = r
∑ xˆ i (k − 1 | k − 1)μi| j (k − 1 | k − 1)
j = 1,..., r
i =1
(18)
P 0 j ( k − 1 | k − 1) = r
∑ μi | j (k − 1 | k − 1){P i (k − 1 | k − 1)
i =1
[
]
+ xˆ i ( k − 1 | k − 1) − xˆ 0 j ( k − 1 | k − 1) • ′ xˆ i ( k − 1 | k − 1) − xˆ 0 j ( k − 1 | k − 1) }
[
]
(19) c.
Mode-matched filtering:
The likelihood functions corresponding to the r filters are given by
Λ j ( k ) = p[ z ( k ) | M j ( k ), Z k −1 ]
(20)
k −1
}
model M j (k ) and previous measurement outputs. The measurement prediction errors and measurement prediction covariance from z (k ) for each filter can be used to find the likelihood Λ j (k ) as below:
Λ j (k ) = Ν[ z (k ); zˆ j [k | k − 1; xˆ 0 j (k − 1 | k − 1)], S j [k ; P 0 j (k − 1 | k − 1)]]
j = 1,..., r
i, j = 1,..., r
(21) (14)
where r is the number of filters, and
pij = P{M j ( k ) | M i ( k − 1), Z k −1}
j = 1,..., r
which is the probability of measurement z (k ) given the
One cycle of the algorithm consists of the following steps:
in effect at k conditioned on Z
(16)
(15)
j where zˆ (•) denotes the estimate of z (k ) by filter j j shown in Eq. (6), S (•) denotes the measurement prediction covariance by filter j shown in Eq. (7), and
N (•) denotes the normal distribution such as
N ( z; z , P ) = 1 | 2πP |−1 / 2 exp( − ( z − z )′P −1 ( z − z )) 2
(22)
where x(k ) denotes the distance in frame k , T is frame time. The true distance data is shown in Figure 1.
d. Mode probability update:
90 80 70
k
μ j ( k ) = P{M j ( k ) | Z } = Λ j ( k )c j r
j = 1,..., r
60
(23)
Distance (meter)
Δ
∑ Λ j ( k )c j
50 Distance 40 30 20 10
j =1
0 0
20
40
60
80
100
120
140
160
Frame Number
where c j is given by Eq. (17). Figure 1: The true distance data between front and host vehicles
e. Estimate and covariance combination: r xˆ ( k | k ) = xˆ j ( k | k ) μ j ( k ) j =1
∑
P(k | k ) =
TTC estimated by Eq. (1) is shown in Figure 2.
r
∑ μ j (k ){P j (k | k ) +
Time-To-Collision
(24) 12
j =1
10
TTC (seconds)
[ xˆ j ( k | k ) − xˆ ( k | k )][ xˆ j ( k | k ) − xˆ ( k | k )]′} EXPERIMENTAL RESULTS SIMULATIONS OF VEHICLE DISTANCE DATA – First, the simulated vehicle distance data is used to verify the model described above. The simulated maneuver is as follows:
8
6
Time-To-Collision
4
2
0 0
20
40
60
80
100
120
140
160
Frame Number
Figure 2: True time-to-collision data plot
•
The initial distance between front and host vehicles is 80 meters;
•
First, the host vehicle is approaching the front vehicle with a relative speed of 0.25 m/frame (27 KPH or 7.5 m/s assuming frame rate is 30 frames per second);
•
When the distance between the two vehicles is 60 meters, the host vehicle starts to accelerate with acceleration 0.001 m/s2 until the two vehicles collide (the distance is 0).
The distance changes in the above maneuver are given by:
0 ≤ k ≤ 80 ⎧⎪ x ( k ) − vT x ( k + 1) = ⎨ 1 2 k ≥ 81 ⎪⎩ x ( k ) − vT − 2 aT
(25)
In reality, there are always distance estimation errors. Assuming the estimation errors can be described by an 2 additive Gaussian noise n ( k ) ~ N (0, σ ) , Figure 3 shows one example of the original and noisy distance data plots. The purpose of the simulation experiment is to verify the performance of the IMM based filter quantitatively.
The measurement noise w(k ) in Eq. (3) should be reasonably true to ensure the Kalman filter accuracy. In this study, we estimate the measurement noise as shown below:
wˆ ( k ) = x ( k ) − xˆ ( k )
(28)
where xˆ ( k ) denotes the estimate of true distance, which is calculated using polynomial curve fitting based on the raw (noisy) distance data. Matlab function “polyfit(x,y,m)” is used to find interpolation polynomial p(x) described above. We have found that the IMM filter gives the satisfactory results for m ≥ 4 . Figure 3: Original and noisy distance data
SYSTEM AND MEASUREMENT NOISE CONSIDERTATIONS IN KALMAN FILTERS - While
implementing CV or CA based Kalman filters, the system and measurement noise need to be estimated. From Eq. (3), the system noise Q for Eq. (12) or Eq. (13) is as below
⎡1 4 ⎢4 T ⎢1 Q = Γσ v 2 Γ′ = ⎢ T 3 ⎢2 ⎢1 T 2 ⎢⎣ 2
1 3 T 2 T2 T
For a constant velocity model,
1 2⎤ T 2 ⎥ ⎥ T ⎥σ v 2 ⎥ 1 ⎥ ⎥⎦
σv
(26)
SIMULATED VEHICLE DISTANCE DATA FILTERING SIMULATION RESULTS - Figure 4 shows an example of the vehicle data filtering simulation results. The simulation results shown in Figure 4(a) indicate that noise is dramatically reduced by using an IMM based filter. The velocity estimates shown in Figure 4(b) are fluctuating around the true velocity is -0.25 m/s for k ≤ 80 , and same are true for TTC estimates shown in Figure 4(c). The mode probabilities shown in Figure 4(d) are dynamically changing to adjust the contributions to the final distance estimates from CV and CA models. Specifically, the solved model probability for CA is getting higher than that for CV while k > 125 which means the distance is changing more like a constant acceleration model.
should be of the
order of the maximum acceleration magnitude a M . A practical range is 0.5a M ≤ σ v ≤ a M [1]. For constant acceleration model,
σv
should be of the order of the
magnitude of the maximum acceleration increment over a sampling period, Δa M . A practical range is
0.5Δa M ≤ σ v ≤ Δa M . In this study, the acceleration a and the acceleration increment Δa are simply
estimated based on the raw distance data as follows
v ( k ) = x ( k ) − x ( k − 1) a ( k ) = v ( k ) − v ( k − 1) Δa ( k ) = a ( k ) − a ( k − 1)
(27)
where x (k ) is the distance average value in sample k to improve the estimation accuracy of
a (k ) and Δa (k ) .
(a) Noisy data (red): SNR = 31 dB, IMM filtered data (green): SNR = 42 dB
COMPARISONS BETWEEN SINGLE MODEL BASED KALMAN FILTER AND IMM ESTIMATOR - Figure 5 shows the filtering results in a simulated mixed maneuver including a constant velocity course first and then a high constant acceleration course. From Figure 5(a), we can see that CV based Kalman filter fails to track the rapid distance change which is caused by high constant acceleration. But IMM based filter is able to capture the rapid distance change by adjusting the mode probability during maneuver changes.
x (k ) by IMM (true value: -0.25 m/frame for k ≤ 80 )
(b) Velocity estimation
(a) Filtered data by constant velocity model based Kalman filter
(c) TTC estimations by IMM (the red curve is true TTC)
(b) Filtered data by CV- and CA- based IMM filter
(d) CV (black) and CA (red) mode probability by IMM Figure 4: Example for IMM based vehicle distance data filtering
(c) IMM mode probability in mixed CV and CA maneuver
(b) Estimated velocity x (k ) by IMM
Figure 5: Kalman filter versus IMM on a mixed CV and CA maneuver data set
REAL DISTANCE DATA FILTERING RESULTS - Figure 6 shows one example of the IMM based filtering results on noisy vehicle distance data solved using the method described in [2]. From Figure 6(a), we see that the noisy distance data is smoothed by IMM filter, although we don’t have a quantitative result because the real distance data is unknown. The estimated velocity and TTC are shown in Figure 6(b), 6(c) which indicate somewhat stable velocity and TTC estimates after the 25th frame. The mode probabilities shown in Figure 6(d) indicate that the filter is dynamically adjusting the output based on mode probabilities of CV and CA models.
(a) Noisy (red) and IMM filtered (green) distance data
(c) TTC estimates using IMM
(d) CV (black) and CA (red) mode probability by IMM Figure 6: Example of IMM based real vehicle distance data filtering
CONCLUSION AND FUTURE WORK
REFERENCES
To improve the accuracy of time-to-collision (TTC) estimation, this study applies Kalman filter to remove noise from the distance data which is used to estimate TTC. By analyzing the scenarios that arise in real road driving, this study proposes to use two different motion models: constant velocity (CV) and constant acceleration (CA), to describe the distance changing dynamics. An interacting multiple mode (IMM) algorithm is used to dynamically merge the outputs from CV and CA based Kalman filters. The experimental results on both simulated and real estimated distance data are found to be satisfactory and indicate that the proposed algorithm does improve the signal-to-noise ratio of distance data.
1. Yaakov Bar-Shalom and X.-Rong Li, Thiagalingam Kirubarajan, Estimation with Applications To Tracking and Navigation, John Wiley & Sons, INC., 2001. 2. Yixin Chen, Manohar Das, Devendra Bajpai, Vehicle Tracking and Distance Estimation Based on Multiple Image Features, The Fourth Canadian Conference on Computer and Robot Vision (CRV2007), Montreal, Canada, May 28-30, 2007, pp. 371-378.
The proposed algorithm is tested using the distance data which is estimated using the multiple image features based vehicle tracking and distance estimation method described in [2]. Nevertheless, the algorithm is also applicable to the distance data filtering in other type of CAS systems such as a radar based CAS system.
CONTACT Yixin Chen, PhD Senior Electronics Systems Engineer Delphi Corporation
[email protected]
