JSAE Review 23 (2002) 6167

Investigation of automatic path tracking using an extended

Kalman lter
Hiroshi Mouria, Shigeki Satoha, Hiroyuki Furushoa, Masao Nagaib
a
Vehicle Research Laboratory, Nissan Research Centre, Nissan Motor Co. Ltd., 1. Natsushima-cho, Yokosuka 237-8523, Japan
b
Department of Mechanical Systems Engineering, Tokyo University of Agriculture and Technology, 2-24-16 Nakamachi, Koganei-shi,
Tokyo 184-8588, Japan
Received 12 April 2001; received in revised form 13 August 2001

Abstract

This paper proposes a lane marker recognition method that uses the steering angle in addition to image information. A Kalman
lter was recongured regarding the yaw motion and lateral motion of lane markers, previously treated as a stochastic process, as
the states of a vehicle model driven on the basis of the steering angle. Driving tests conducted with an actual vehicle veried that this
method provides good tracking at the time of steering input and avoids misrecognition of lane marker candidate points in inclement
weather. r 2002 Society of Automotive Engineers of Japan, Inc. and Elsevier Science B.V. All rights reserved.

1. Introduction bench test simulations and driving tests conducted with

An investigation was made of a method for estimat-
ing the road conguration and the state quantities of
a vehicle based on detection of forward lane markers
by a vehicle-mounted CCD camera. Pitching, bouncing
and other vehicle motion induced by the road curvature
or by road surface irregularities were regarded as a
steady stochastic process, and the vehicle motion
resulting from a steering angle input was treated as a
deterministic state based on a vehicle model. Under
these premises, the movement of lane marker candidate
points in images captured with the vehicle-mounted
camera was expressed as a combination of stochastic
behavior induced by disturbances applied to the vehicle
and deterministic behavior corresponding to the steering
angle.
A Kalman lter congured on the basis of the
foregoing assumptions made it possible to secure good
tracking of fast vehicle movements at the time of
steering input while at the same time reducing mis-
recognition of lane markers due to road surface noise.
An extended Kalman lter was used to estimate the road
curvature and state quantities of the host vehicle, taking
into account the nonlinearity that occurs when CCD
camera images of three-dimensional space are converted
to two-dimensional plane images. The validity of this
estimation method was investigated and veried in
an actual vehicle.
2. Behavior of lane marker candidate points
Fig. 1 shows time histories of he behavior of the
coordinates of a lane marker candidate point in images
taken with the CCD camera. It is seen in the left-hand
graphs that the movement of the lane marker candidate
point is moderate under gradual lane-keeping steering,
similar to a condition of automated driving. In contrast,
the right-hand graphs show that in response to a sudden
steering input, the lane marker candidate point moves at
high speed, which would not occur in the case of
automatic steering. Additionally, even during gradual
steering, there are times when the coordinates of the lane
marker candidate point change suddenly, which could
be caused by misrecognition of lane markers due to road
surface noise or some other factor. An example of
misrecognition of a lane marker candidate point on a
rain-slick road surface is shown in Fig. 2.
Consider that a state estimator is congured such that
the movement of lane marker candidate points is treated
entirely as stochastic variation. If the dynamic gain of
the estimator is set high, the values estimated at the time
of steering intervention by the driver will closely follow
the real values, but frequent misrecognition of candidate

0389-4304/02/$22.00 r 2002 Society of Automotive Engineers of Japan, Inc. and Elsevier Science B.V. All rights reserved.
PII: S 0 3 8 9 - 4 3 0 4 ( 0 1 ) 0 0 1 7 1 - 0 JSAE20024010
62 H. Mouri et al. / JSAE Review 23 (2002) 6167

Fig. 1. Behavior of a candidate point of lane markers.

Fig. 3. Denitions of coordinate axes.

Fig. 2. Behavior of lane marker candidate points.

points will occur under inclement weather conditions.

However, setting the dynamic gain low gives rise to the
problem that the estimated values will lag behind the
real values at the time of steering intervention by the
driver.
In this investigation, a method of using steering angle
information was employed to resolve these issues,
focusing on the fact that the dynamic characteristics of
movement by lane marker candidate points due to
steering by the driver dier from the dynamic char-
acteristics of their movement induced by the road
conguration.
3. Road and vehicle models
The coordinates are dened as shown in Fig. 3. The
notations X; Y and Z denote three-dimensional
coordinates (with the center of the lens of the vehicle-
mounted CCD camera as the origin) in absolute space,
and x and y represent coordinates in a two-dimensional
plane in an image taken by the CCD camera. The
relationships between the XYZ coordinates and the xy
Fig. 4. Road model (plan view).
coordinates are given by Eqs. (1) and (2):
x fX=Z; 1
y fY=Z; 2
where f is a parameter of the camera lens.
The road model was formulated under the following
assumptions:
(1) The road surface is a plane without any long-
itudinal or lateral gradient.
(2) The road has no large curves.
Based on these assumptions, the road was modeled in
a plan view with a quadratic equation (Fig. 4) and in a
side view (Fig. 5) with a linear equation. It should be
noted, however, that these assumptions were made for
the sake of simplifying the model. The state quantities
can also be estimated under more general conditions by
 H. Mouri et al. / JSAE Review 23 (2002) 6167 63

cf C f Cf
b11 ; b31  ;
IN mN
where cf cr is the distance from the front (rear)
wheels to the vehicles center of gravity, Cf Cr the
cornering power of the front and rear wheels (for
two wheels), m the vehicle mass, y the steering angle,
V the vehicle velocity, I the moment of yaw inertia of
the vehicle, N the steering gear ratio and f the yaw
rate.
Fig. 5. Road model (side view). The behavior of the stochastic state variables x, such
as the road curvature r; pitching angle Z; amount of
bounce h and various disturbances, was approximated
increasing the orders of the model. From Figs. 4 and 5, as a linear system driven by white noise and expressed
the following equations are obtained: as [1,2]
r 2 W x lx qln; 7
X Z  fr Z  ycr  iW; 3
2 2
where l is used to set the frequency region of
Y ZZ  h; 4 disturbances applied to the vehicle, q corresponds to
the standard deviation of process noise, and n is
where r is the road curvature, fr the vehicles yaw angle normalized white Gaussian noise.
relative to the center line of the road, ycr the lateral 2 3
displacement at the vehicles center of gravity relative to f.
6 7
the center line of the road, i the lane marker number (left 6 fr 7
6 7
side=0; right side=1), W the lane width, Z the distance 6 y. 7
6 cr 7
ahead of the vehicle from the lens center, X the distance 6 7
6 ycr 7
to the left side of the vehicle from the lens center, Z the 6 7
6 r 7
vehicle pitching angle and h the height from the road 6 7
6 7
surface to the lens center. 6 Z 7
6 7
From Eqs. (1)(4), Eq. (5) is obtained as the out- 6 7
6 h 7
put equation when an extended Kalman lter is 6 7
4 5
6M 7
formulated. The x-coordinate is calculated with this
equation using the state quantities of the vehicle, road F
conguration and the y-coordinate in the CCD camera 2 3
a11 a12 a13 0 0 0 0 1=I 0
image. 6 1

 6 0 0 0 V 0 0 0 0 77
ycr  W=2 W f 2 hr 6 7
x i y f Z  f fr : 5 6 a31 a32 a33 0 V 2 0 0 0 1=m 7
h h 2y f Z 6 7
6 7
6 0 0 1 0 0 0 0 0 0 7
The objective of this investigation was to estimate the 6 7
6 0 0 0 0 lr 0 0 0 0 7
road curvature and state quantities of the vehicle from 6 7
6 7
6 0 0 0 0 0 lZ 0 0 0 7
the pair of (x; y) coordinates and the steering angle. 6 7
Based on a general two-wheel model, the vehicle model 6 0 0 0 0 0 0 lh 0 0 7
6 7
was described with the following equations of state: 6 7
4 0 0 0 0 0 0 0 lM 0 5
f. a11 f a12 f a13 yc b11 y; 0 0 0 0 0 0 0 0 lF
2 3 2 3
y.c a31 f a32 f a33 yc b31 y; 6 f b11
6 7 6 0 7
f
6 r7 6 7
where each of the coecients is determined by the 6 7 6 7
6 ycr 7 6 b31 7
vehicle specications. 6 7 6 7
6 7 6 7
6 ycr 7 6 0 7
c2f Cf c2r Cr cf Cf  cr Cr 6 7 6 7
a11  ; a12  ; 6 7 6
6 r 7 6 0 7y
7
VI I 6 7 6 7
cf Cf  cr Cr 6 Z 7 6 0 7
a13  ; 6 7 6 7
VI 6 h 7 6 0 7
6 7 6 7
6 7 6 7
cf Cf  cr Cr Cf Cr Cf Cr 4M5 4 0 5
a31  ; a32 ; a33  ; F 0
mV m mV
64 H. Mouri et al. / JSAE Review 23 (2002) 6167
2 3
0 0 0 0 0
6 0 Pk 1 As MkATs GQGT ; 14
6 0 0 0 0 7 7
6 7 2 3
6 0 0 0 0 0 7 nr Mk PkfI  CT CPkC T R1 CPkg; 15
6 7
6 7 6 7
6 0 0 0 0 0 7 6 nZ 7
6 7 6 7 where Mk is the covariance of the state determined by
6
6 qr lr 0 0 0 0 7 7
6 nh 7 :
6 7 8 the process, Pk the covariance of the state after
6 7 6 7
6 0 qZ lZ 0 0 0 7 4 nM 5 obtaining the observed value and Kk the Kalman lter
6 7
6 0 0 qh lh 0 0 7 nF gain.
6 7
6 7 @gxs k; yk
4 0 0 0 qM l M 0 5
CD :
0 0 0 0 qF lF @xs

The notations M and F represent the yaw moment

and lateral force of disturbances applied to the vehicle.
nr nZ nh nM nF T assumes the presence of white Gaussian 4. Conrmation of eectiveness of proposed method
noise. The notations qr ; qZ; qh ; qM ; qF express the based on actual vehicle data
standard deviation of the disturbance that drives each
state variable. Eq. (8) can be rewritten as noted below by Without the use of steering angle information, the
simplifying the discrete expressions while preserving the equation of state is expressed as shown in Eq. (16) when
state variables: the yaw motion and lateral motion of the vehicle are also
regarded as stochastic states. The Kalman lter cong-
xs k 1 Ax xs k Gs vk; 9 ured on the basis of Eq. (16) is referred to as the previous
lter. However, the Kalman lter proposed here, which
xs k fT k f r k yTc k yTc k rT k ZT k is based on Eq. (8) and makes use of steering angle
hT k M T k F T kT : information, is referred to as the proposed lter. The
dierence between the two lies in whether the dynamic
As indicated by Xs ; the subscript s has been added characteristics of the vehicle are considered or not.
to the state variable vectors to avoid confusion with the 2 3
x-coordinate of the CCD camera images. The output x f.
6 7
is the value of the x-coordinate of the pixels in the 6 f r 7
images taken with the CCD camera. The out- 6 7
6 7
6 y.cr 7
put equation is a nonlinear function of the state 6 7
6 ycr 7
variables, as expressed in Eq. (5). The values of the 6 7
6 7
(x; y) coordinates are specically the coordinates of 6 r 7
6 7
the white line candidate points. A total of ten points 6 Z 7
4 5
were selected for the lane makers on the right and left
sides. The value of the y-coordinate was kept constant. h
2 32 3
The output equation for the jth ( j=110) x coordinate lf 0 0 0 0 0 0 f
can be written as 6 76 7

 6 1 0 0 0 V 0 0 7 6 fr 7
ycr  W=2 W 6 76 7
6 0 0 7 6 7
7 6 ycr 7
2
xj i yj f Z 6 0 lycr 0 V 0
h h 6 76 7
6 0 0 1 0 0 0 0 7 6 ycr 7
f 2 hr 6 76 7
6 0 0 7 6 7
 f fr : 10 6 0 0 0 lr 0 76 r 7
2 y j f Z 6 76 7
4 0 0 0 0 0 lZ 0 54 Z 5
The following expression is obtained by simplifying 0 0 0 0 0 0 lh h
Eq. (10): 2 3
qf lf 0 0 0 0
x gxs ; y: 11 6 72 3
6 0 0 0 0 0 7 nf
The term gxs ; y expresses the nonlinear function of 6 76 7
6 0 7
xs ; y: 6 0 lycr
qycr 0 0 766 7
nycr
7
6 7
The Kalman lter is congured with the following
6 0 0 0 0 0 76 6 nr 77: 16
6 76 7
equations [3]: 6 0 0 qr lr 0 7
0 7 4 nZ 5
6
6 7
x# s k 1jk As x# s kjk  1 Bs yk 4 0 0 0 qZ lZ 0 5 nh
Kkfxk  gx# s kjk  1; ykg; 12 0 0 0 0 qh l h
The performance of each lter was evaluated by
Kk As PkC T CPkC T R1 ; 13 comparing the yaw rate, measured with an onboard
 H. Mouri et al. / JSAE Review 23 (2002) 6167 65

gyro, and the value estimated by the lter. Two

evaluation conditions were used:
(1) with a fast steering angle input,
(2) with misrecognition of lane markers due to road
surface noise. Lane marker candidate points were
detected by an edge processing technique as the basic
procedure. Consequently, under condition (2), the
behavior of the lane marker candidate points became
discontinuous, as shown in Fig. 6, because the contrast
produced by road surface noise due to rain or some
other factor gave rise to misrecognition.
The results that were estimated when the dynamic Fig. 6. Coordinates of lane marker.
gain of the previous lter was set high are shown in
Fig. 7a. Under condition (1) with a fast steering angle
input, the estimated yaw rate followed the actual result
with good responsiveness. However, with this lter,
estimation error with a large amplitude occurred when
there was a high level of road surface noise like that in
the case of condition (2).
Fig. 7b, on the other hand, shows the results that were
estimated when the dynamic gain of the previous lter
was set low. In this case, the amplitude of the estimation
error when misrecognition occurred was smaller com-
pared with the results in Fig. 7a, but the yaw rate could
not be estimated accurately because the dynamic gain of
the estimator was set too low.
As these results indicate, the previous lter required a
trade-o between its ability to follow the actual value
(estimation speed) and the amplitude of estimation error
when misrecognition occurred (noise resistance).
Fig. 8 shows the results obtained with the proposed
lter under the same conditions (1) and (2) used for
evaluating the previous lter. The dynamic gain of the
proposed lter was set at a constant level regardless of
the evaluation conditions. From the results in this gure,
it is clear that the trade-o problem seen with the
previous lter was resolved by the proposed lter as a
result of using the steering angle to model vehicle
behavior.

5. Control system application

This section proposes a new automatic path tracking

system that integrates the estimator and control system
explained in the preceding sections. The previous
automatic path tracking system was congured accord-
ing to the general approach, whereby the image Fig. 7. (a) Validation of previous lter (with higher gain). (b)
processing system for detecting lane markers and Validation of previous lter (with lower gain).
estimating the host vehicles lateral displacement at a
forward observed point was constructed separately from
the system for controlling steering on the basis of that observed point, was ineective. In other words, it is
information. However, in trying to congure the more rational and eective for the image processing
optimum steering control system, it was found that a system directly to estimate and use the vehicle state
control system which uses estimates of intermediate variables needed for optimum steering control. In short,
variables, such as the lateral displacement at a forward a new method is proposed here for achieving optimum
66 H. Mouri et al. / JSAE Review 23 (2002) 6167

Fig. 9. Example of image data used in simulation.

Fig. 8. Validation of proposed lter.

control by estimating the vehicle state variables directly

from the coordinate values in the images taken with the
CCD camera.
An H2 controller can easily be congured by
expanding Eq. (8) in its present form to create a
generalized plant. However, because the output equa-
tion is in a nonlinear form as indicated in Eq. (5), the
extended Kalman lter described in Section 4 is used in
estimating the state quantities of the vehicle. For the
regulator (state feedback block), a feedback gain is used
that makes it possible to observe all the state quantities.
As a result, even though the system is nonlinear, it is
possible to minimize by approximation the H2 norm
from disturbance w to the controlled variable z:
Fig. 10. Simulation results (automatic path tracking using H2 con-
x Ax B1 w B2 u; troller and proposed Kalman lter).
z C1 x D11 w D12 u;

f ycr ycr ;
x f z z1 ; yT ;
w r M FT ; u y; weighting of the lateral displacement. In this way,
frequency adjustment is also possible in addition to
2 3 2 3 scalar weighting. The feedback gain is given as K
a11 a12 a13 0 0 1=I 0
6 1 BT2 H and H is the positive solution to the following
6 0 0 07
7
6 V
6 0 0 77
A6 7; B1 6 7; Riccati equation:
4 a31 a32 a33 05 4 V 2 0 1=m 5
0 0 0 0 0 0 0 AT H HA  HB2 BT2 H C1T C1 0 18
2 3
b11 Fig. 9 shows an example of the lane markers used in a
6 0 7 simulation of a vehicle traveling along a leftward curve.
6 7
B2 6 7; The shape and position of the lane markers projected
4 b31 5
onto the screen are suitably changed according to the
0 road curvature and state quantities of the host vehicle,
" # " #
0 0 0 Q1=2 0 similar to the situation for an actual vehicle. The
C1 ; D12 ; D11 023 : simulation results are shown in Fig. 10, which compares
0 0 0 0 1
the values estimated with the Kalman lter and the real
17 state quantities used in the simulation. This example is
Equation (17) congures a generalized plant that is for automatic steering of the host vehicle as it enters a
equivalent to the optimum regulator and Q is the curve without any easement curve. The actual and
 H. Mouri et al. / JSAE Review 23 (2002) 6167
estimated state quantities show good agreement even
though a relatively fast steering angle was input.
6. Conclusions
This paper proposed a new path tracking control
system that incorporates an image processing system
for detecting forward lane markers by means of a
vehicle-mounted camera. The following new tracking
method is proposed from the standpoint of modern
control theory:
(1) The new image processing system proposed here
incorporates a method for considering the host vehicles
dynamic characteristics into an extended Kalman lter,
which is used for estimating the road curvature and state
quantities of the vehicle. By using steering angle
information, this method makes it possible to provide
both good convergence of the estimated values and
improved anti-noise performance. The attainment of
67
both capabilities was conrmed by theoretical calcula-
tions and the results of actual driving tests.
(2) An optimum steering control system is also
proposed which is constructed such that the image
processing system directly estimates the dynamic char-
acteristics of the host vehicle and uses the results for
steering control. The eectiveness of this system was
veried by computer simulations.
References
[1] Mouri, H., Furusho, H., Research on automatic lane tracking
using linear quadratic (LQ) control: control procedure for a curved
path (II) (in Japanese with English summary), Proc. Spring
Convention of JSAE, Vol. 103, No. 972 (1997).
[2] Mouri, H., Furusho, H., Research on lane tracking control (in
Japanese with English summary), Trans. JSAE, Vol. 30, No. 2,
(1999).
[3] Kimura, H., Digital Signal Processing and Control (in Japanese),
Shokodo, Tokyo, 1982.