Global positioning system roadside integrated precision positioning system

[0002] This application claims the benefit of provisional application No. 60/174,370 filed Jan. 5, 2000.

[0003] 1. Field of the Invention

[0004] The present invention relates to the field of vehicle positioning systems that can be used to ascertain whether a vehicle is positioned within a lane on a roadway. More specifically, the present invention is directed to a vehicle positioning system that integrates global positioning system (GPS) data with other navigational data to determine in a real-time, robust and precise manner a lane position of the vehicle on a mapped roadway for lane departure detection.

[0005] 2. Description of the Related Art

[0006] Many different on-road vehicle systems exist or are being developed to address individual applications such as lane-keeping, lateral collision avoidance, intersection collisions, route planning, traffic management, collision notification, automated control, etc. Each of these systems varies in performance and implementation challenges. Both commercial and government activities continue to address the problem of combining systems designed for specific applications to provide low cost, integrated vehicle systems which can significantly increase driver and vehicle safety. GPS has significant potential for enabling a variety of transportation user services.

[0007] Prior art systems have used a variety of methods to determine and maintain the position of a vehicle with precision. One type, through the use of optical systems, uses cameras and visualization techniques to follow lane boundaries. The benefits of such optical systems are that they have a relatively low cost, are based on existing technology and use existing lane markers. Optical systems have problems related to reliability due to paint differences, weather, exit lanes, etc. Other methods to determine and maintain the position of a vehicle with precision are magnetic sensor systems. In such a system, the vehicle follows magnets or magnetic pavement marking tape along lane boundaries. Such systems have a relatively low cost since they use low cost magnetic sensors and electronics and are largely not adversely impacted by weather. However, such systems require a great deal of infrastructure and upkeep, have reliability issues due to missing sensors and are impacted by the presence of large metallic objects in the area, as well as by trucks and buildings.

[0008] Other prior art systems used to determine and maintain the position of a vehicle with precision utilize driver based sensors. Such systems detect drowsiness from driver characteristics such as eye movement and/or wheel motion. Such systems have the benefit in that they can often identify the state of the driver prior to lane departure but are hampered by reliability problems due to operator differences and false alarms and do not benefit from an actual road position. RF systems are also used to detect lane position through the input of RF signals along lane/road boundaries. The RF systems are useful in all weather environments but require a great amount of infrastructure and upkeep required for the RF emitters along the road.

[0009] GPS and Differential GPS (DGPS) have also been used in prior art systems to determine and maintain the position of a vehicle with precision. In such prior art systems, the actual vehicle position is compared to surveyed lane boundaries through the use of GPS data. Such systems have the benefits of being useful in all weather, use existing lane markers and work with other systems that also use GPS data However, in prior art systems, the survey of the roadway is still needed and more importantly there are reliability issues due to signal blockage. Additionally, the application of DGPS using a low cost GPS receiver can result in position accuracy on the order of 1-5 meters. While such a level of accuracy is adequate for many applications, in lane-keeping approaches and other applications that require greater precision, systems that rely only on GPS or DGPS are not adequate to prevent imminent lane departure.

[0010] Accordingly, for the above reasons, the present invention is directed to a real-time, high-precision, DGPS-based, robust (due to the use of other navigation aids in addition to DGPS), automotive navigation system that can support a wide range of highway traffic applications. Specifically, the GPS based system is used to precisely monitor a vehicle's location, in real time, relative to road lane boundaries. The present invention is directed to a GPS Roadside Integrated Precision Positioning System (GRIPPS).

[0011] A first embodiment of the present invention is directed to a real-time integrated navigation system for a vehicle. The system includes a GPS receiver, connected to a first antenna and receiving GPS data from satellites and outputting GPS position data, and a communications link, connected to a second antenna and to the GPS receiver, receiving range and carrier phase measurements from at least one base station. The system also includes navigation aids which provide relative position data of said vehicle, and a Kalman filter, connected to the output of the GPS receiver and the navigation aids, that integrates the GPS position data and the relative position data and outputs smoothed position data

[0012] In the system of the first embodiment the smoothed position data compensates for degradation, blockages or communication dropouts of the GPS data from the satellites. The navigation aids of the system can include at least one, and, preferably, at least two or more, of a distance measurement, e.g., odometer measurements or anti-lock braking system (ABS) wheel turns, heading measurements, and tilt measurements. Distance and heading measurements are preferred if only two navigation aids are used. More specifically, the navigation aids can include at least one, and, preferably, at least two or more, of anti-lock braking system wheel turns, electronic compass heading and pitch, and map vertical height that provides for the generation of a high rate, robust reference relative position. The real-time integrated navigation system for a vehicle provides smoothed position data that has a 2-cm position accuracy when GPS updates from good phase measurements are available.

[0013] In addition, the system of the first embodiment can include a lane departure module that receives the smoothed position data and utilizes stored map data to output a signal that is related to imminent lane departures of the vehicle. The system may have a spread spectrum radio modem as a part of its communications link and the second antenna can be a 900 MHz communications antenna to facilitate communications with the ground stations; other, better communications systems are commercially available.

[0014] A second embodiment of the present invention is directed to a method of providing real-time navigation data to a vehicle, having the steps of: receiving GPS data from satellites via a first antenna connected to a GPS receiver located on the vehicle and then receiving range and carrier phase measurements from at least one base station via a communications link connected to a second antenna and sending the range and carrier phase measurements to the GPS receiver. The method further includes the steps of outputting GPS position data from said GPS receiver to a Kalman filter and querying navigation aids to provide relative position data of said vehicle and providing the relative position data to said Kalman filter. GPS position data and the relative position data are integrated, using said Kalman filter, and the smoothed position data for the vehicle is output.

[0015] The method compensates for degradation, blockages or communication dropouts of the GPS data from the satellites. In addition, the queried navigation aids can include components providing at least one, and, preferably, at least two or more, of a distance measurement, e.g., odometer measurements or ABS wheel turns, heading measurements, and tilt measurements. Distance and heading measurements are preferred if only two navigation aids are used. More specifically, the components can provide at least one, and, preferably, at least two or more, of anti-lock braking system wheel turns, electronic compass heading and pitch, and map vertical height that provides for the generation of a high rate, robust reference relative position.

[0016] The method may also include providing the smoothed position data to a lane departure module and computing imminent lane departures of the vehicle based on stored map data and the smoothed position data. Preferably, the smoothed position data has a 2-cm position accuracy.

[0017] The method is also applicable where the communications link receives range corrections via a spread spectrum radio modem and the second antenna is a 900 MHz communications antenna.

[0018] The features of the disclosed method and system will become more readily apparent and may be better understood by referring to the following detailed description of illustrative embodiments of the present invention, taken in conjunction with the accompanying drawings. FIGS. 1 to 6 show embodiments of the present invention, wherein

[0019] FIG. 1 illustrates the overall configuration of the GPS Roadside Integrated Precision Positioning System (GRIPPS);

[0020] FIG. 2 illustrates an integrated system architecture for GRIPPS;

[0021] FIG. 3 illustrates a schematic showing the integration of data to determine lane departure of a vehicle;

[0022] FIG. 4 illustrates a schematic representation of navigational aids used in the system;

[0023] FIG. 5 illustrates a schematic representation of a specific embodiment of the system of the present invention; and

[0024] FIG. 6 illustrates experimental results of lane deviations of a vehicle as determined by the system of the present invention.

[0025] Embodiments of the present invention will now be described with reference to FIGS. 1 to 6.

[0026] Standard commercial products for GPS support civilian Coarse/Acquisition (C/A) code GPS which provides position accuracy on the order of 30-50 meter Circular Error Probability (CAP), due primarily to Selective Availability (SA). The application of Differential GPS (DGPS) using a low cost GPS receiver can result in position accuracy on the order of 1-5 meters. DGPS involves the broadcasting of navigation data and measurements or corrections from a surveyed base station. This approach can mitigate the effect of common error sources.

[0027] Current efforts exist to provide an infrastructure (i.e. WAAS, WADGPS, US Coast Guard, Minnesota DOT, etc.) for the transmission of differential GPS corrections. In order to get high position accuracy (2-19 cm), a system based on good signal phase measurements and cycle ambiguity resolution is required to achieve positioning accuracy of a few centimeters. Furthermore, additional navigation data at a higher update rate must be provided; separation between surveyed ground stations must be minimized; and the GPS receiver would require the capability to minimize multipath and noise. However, in a dynamic environment, consistency of high accuracy GPS is dependent on many factors including: receiver quality, distance from base station, reliability of the communications RF link, satellite geometry, blockage of GPS signals, RF and GPS antenna placement/multipath, etc.

[0028] The potential ability to support multiple transportation applications, while taking advantage of existing infrastructure, makes precise vehicle positioning using GPS an important technology area to pursue. As noted above, however, prior art systems have been unable to take advantage of these benefits.

[0029] A wide range of transportation applications can be supported with a single, configurable on-board vehicle system. Some of the applications, such as route planning, collision notification, and traffic management require easily achieved position accuracies on the order of 10-30 meters. However, applications such as lane-keeping, collision avoidance, impaired driving detection, and automated vehicle control require real-time precise positioning and a precision reference map. For example, the lane-keeping application requires accuracy on the order of a few centimeters to identify imminent lane departures early enough such that the operator can take preventative measures. If multiple vehicles applied a precision positioning system with two-way communications, their positions could be broadcast to other vehicles in the immediate vicinity. These positions could be tracked by software on-board the vehicles to support warning the operators of potential collisions.

[0030] Monitoring accurate vehicle positions over time and comparing to nominal driver behavior could provide a measure of driver effectiveness (i.e. identify a drowsy or impaired driver). For real-time vehicle control, the precise position information could be used with surveyed map data and vehicle control actuators to support navigation and, hence, control of the vehicle. Even though the same accuracy is not required, the position information and two-way communications could be used to support route planning, collision notification and traffic management as well.

[0031] A properly designed, in-vehicle, GPS-based system can support all functions at a high level of accuracy and provide a robust, all weather alternative to other sensor systems being considered (magnetic, vision-based, radio frequency (RF) transponders). An integrated GPS system appears to offer the capability to support systems envisioned for “intelligent” vehicles of the future, —private automobiles, commercial and transit vehicles.

[0032] The development activities associated with the present invention concentrated on the real-time system instrumentation and software for an Instrumented Vehicle (IV). These activities can be broken up into system architecture, system development, Kalman filter, and software methodology.

[0033] The general architecture of the present invention is illustrated in FIG. 1. The IV is illustrated as a mobile unit 10, which contains a GPS unit and other processing portions of the system. The IV receives signals from GPS satellites 20 and communication relays 30 along the roadway. The signals sent to the communications relays are coordinated by base stations 40 that are also in communication with the GPS satellites 20. Through the system, the position of the vehicle can be determined with a high degree or accuracy and departures from the lane of the roadway upon which the vehicle is traveling can be determined

[0034] A specific embodiment of the present invention as it relates to determination of lane departure is illustrated generally in FIG. 2. The IV is illustrated as a ROVER portion 115. The IV has a GPS receiver 100 and communication module 110 that provides data to the GPS receiver 100. The communications module receives data from a base station that also has a GPS receiver 101 and a communication module 111. The output of the GPS receiver 100 in the IV is sent to the Kalman filter 130. The filter integrates data from other navigation aids 120 to determine an output that signals whether a lane departure has been determined by the lane departure module 140.

[0035] An integrated system architecture, using DGPS 100 and navigation aids 120 to calculate real-time vehicle position, is illustrated in FIG. 3. The real-time data collection system was developed on the IV to acquire DGPS, odometer, heading, tilts, inertial navigation measurement, gyroscope, and video camera data. FIG. 3 also shows the outputs of the different portions of the system, that will be discussed in greater detail below.

[0036] The architecture developed for the system of the present example of the invention is based on convenience and utilization of existing hardware. A DGPS-based system was chosen over Optical, Magnetic, and RF systems for reasons mentioned in the previous section. First, the performance of several commercial GPS receivers were evaluated for ability to provide 2-cm accuracy. Examination of commercial GPS receiver performance showed that there are several receiver systems that offer differential position accuracy to a few centimeters. The GPS-based inertial navigation system of the present invention was designed around a DGPS system developed by ASHTECH, but other off-the-shelf DGPS systems can also be used.

[0037] In terms of communications options, for communicating between the vehicle and the base stations, options such as VHF/UHF radio, FM-subcarrier, cellular phone, Cellular switched-circuit data, Iridium, and Location and Monitoring Service (LMS) are all applicable to the present invention. The LMS communication link was selected in the preferred embodiment, despite the fact that cellular phone and VHF/UHF communications are proven technologies and are capable of transmitting data at greater distances. The reference stations used transmit L1 and L2 code and carrier phase data at a 2 Hz update rate to the mobile unit through a LMS 900 MHz RF antenna and communication modem. The Instrumented Vehicle (IV) was outfitted with two computers, inertial and vehicle sensors, and a differential GPS system.

[0038] In a preferred embodiment, the GPS system includes a GPS receiver, a choke ring antenna, a spread spectrum radio modem, and a 900 MHz antenna In the preferred embodiment, an ASHTECH Z-12 GPS receiver served as the GPS receiver. The present invention finds utility with small, high-quality GPS antennas, which are generally available; however, choke ring antennas provide improved multipath mitigation, in the preferred embodiment.

[0039] The moving receiver uses navigation data and measurement data from the base station through the RF communications link, and data corrections for applicable satellites are computed by the receiver to accurately determine the vehicle position. High precision positioning is accomplished because the receiver employs sophisticated processing (which can include L1/L2 codeless, narrow correlators, multipath/cycle slip mitigation, internal ambiguity fixing and differential carrier phase ranging techniques). Based on theoretical performance analysis, the highest accuracy is achieved when base to mobile separation is within a few kilometers.

[0040] In this implementation, the system directly applies the position information as provided by the receiver. As shown in FIG. 4, GPS position and velocity data, select navigation aids (odometers 201, vehicle heading 202, and vehicle tilts 203) and vertical map measurements (from the surveyed reference map 206) were integrated 204 using an Extended Kalman filter to smooth through GPS signal dropouts. This serves to enable vehicle navigation during GPS blockage with graceful performance degradation. Data from the navigation aids were analyzed to determine the optical configuration of sensors to smooth through data dropouts.

[0041] In testing, data was collected to evaluate several GPS hardware configurations to determine an initial system approach that would increase reliability of the position data. A video camera was also used to view the lane markings in order to provide an independent observation of the lane departures and to assist in mapping the road boundaries. A user interface for lane departure and warning capability was developed.

[0042] The data acquisition hardware will now be addressed. The Instrumented Vehicle (IV) data acquisition system is built around a processor equipped with several ports used to acquire serial and analog sensor outputs. This system is also equipped with a network interface to the Kalman Filter Processing System, which is run, preferably, on a separate processor. This processor accepts analog signals from the inertial measurement unit and a pulse train representing wheel turns from the Anti-Lock Braking System (ABS).

[0043] The Kalman filter utilizes software to process incoming signals. Upon receiving data, the Kalman filter software implements the filter on the real-time data and subsequently returns data indicating lane deviation, filter process status and whether the IV is in the reference map location.

[0044] An equipment stack contains navigation aids and the ABS interface. The wheel turns are sensed from the ABS, while heading and pitch data are measured from an electronic compass sensor module. The navigation aids also include an Inertial Measurement Unit, which provides linear accelerations and angular rates, and a fiber optic gyroscope.

[0045] In an exemplary embodiment, illustrated in FIG. 5, the GPS based inertial navigation system instrumentation installed in the Instrumented Vehicle is made up of the following:

[0046] Data Acquisition System Computer (PENTIUM 180 MHz computer), 401;

[0047] Kalman Filter Processor/Real Time video Computer (PENTIUM 180 MHz computer), 402;

[0048] CROSSBOW DU6 Inertial Measurement Unit, 403;

[0049] PRECISION NAVIGATION TCM2 Electronic Compass with tilt, 404;

[0050] ANDREW 225140 AUTOGYRO Fiber Optic Gyroscope, 405;

[0051] ASHTECH Z-12 DGPS Receiver, 406; and

[0052] ASHTECH Spread Spectrum Radio Modem, 407.

[0053] The exemplary embodiment also includes interface modules that allow communication between the constituent parts. While the above constituent parts of the system have been used in a particular embodiment, the present invention is not so limited. The range of applicable components of the real-time integrate navigation system are discussed above.

[0054] The Kalman filter software will now be discussed. An eight-state Extended Kalman filter (E-KF) with GPS and navigation aids is used to estimate the vehicle position/velocity errors. An estimate and covariance propagation is performed every update time. A measurement update occurs when there is either GPS, height, or both measurements available. The state vector is: $\begin{array}{cc}\delta x=\left[\begin{array}{c}\delta N\\ \delta E\\ \delta D\\ \delta \mathrm{SFW}\\ \delta \psi \\ \delta \theta \\ \delta \mathrm{SF}\theta \\ \delta H\end{array}\right]\{\begin{array}{c}\mathrm{North}\mathrm{vehicle}\mathrm{position}\mathrm{error}\left(m\right)\\ \mathrm{East}\mathrm{vehicle}\mathrm{position}\mathrm{error}\left(m\right)\\ \mathrm{Down}\mathrm{vehicle}\mathrm{position}\mathrm{error}\left(m\right)\\ \mathrm{Wheel}\mathrm{turn}\mathrm{scale}\mathrm{factor}\mathrm{error}\left(\mathrm{unitless}\right)\\ \mathrm{Azimuth}\mathrm{error}\left(\mathrm{deg}\right)\\ \mathrm{Pitch}\mathrm{error}\left(\mathrm{deg}\right)\\ \mathrm{Pitch}\mathrm{scale}\mathrm{factor}\mathrm{error}\left(\mathrm{unitless}\right)\\ \mathrm{Height}\mathrm{error}\left(m\right)\end{array}& \left(1\right)\end{array}$

[0055] where the vehicle position is in the North-East Down (NED) local frame defined by the reference (base station) position.

[0056] After each measurement update, the E-KF accumulates whole value estimates for position (r), navigation (α), and velocity (v) by the following equations:

[0057] {circumflex over (r)}_i+={circumflex over (r)}_i−+δ{circumflex over (r)}_i+{circumflex over (α)}_i+={circumflex over (α)}_i−+δ{circumflex over (α)}_i+v_i+=v_i+δ{circumflex over (r)}_i+/Δtδ{circumflex over (x)}=0,  (2)

[0058] where δr are the first three elements, and δα are the last five elements of the state vector, δx. The transitions are defined by $\begin{array}{cc}\left[\begin{array}{c}\delta \stackrel{.}{r}\\ \delta \stackrel{.}{\alpha }\end{array}\right]=F*\left[\begin{array}{c}\delta r\\ \delta \alpha \end{array}\right],\mathrm{where}F=\left[\begin{array}{cc}0& {\stackrel{\_}{F}}_{r\alpha }\\ 0& 0\end{array}\right]& \left(3\right)\end{array}$

[0059] The transition matrix in the filter is approximated by:

[0060] Φ_i,j−1=I+F*Δt+½*F²*Δt²  (4)

[0061] where

[0062] {overscore (F)}_rα=[{dot over (d)}₁*B₁|−[{overscore (V)}_x]*A₃|−[{overscore (V)}_x]*A₂|−{overscore (θ)}*[{overscore (V)}_x]*A₂|0], T₁=(−{overscore (ψ)})₃, T₂=(−{overscore (θ)})₂, T_NB2=[T₁*T₂]  (5)

[0063] (A_j, B_j) are the columns of (T₁, T_NB2) and {dot over (d)}₁is defined later in this section. Note that A₂=B₂and A₃=[0, 0, 1]^T. The diagonal process noise matrix, which is based on judgement and testing, is Q=[q_k²], where the nominal q_k=[0.5, 0.5, 0.4, 10⁻⁴, 0.5, 0.1, 0.0, 0.001]. The diagonal prior uncertainty matrix is P₀=[p_k²], where p_k=[10⁺⁴, 10⁺⁴, 10⁺⁴, 0.04, 20, 3, 10⁻⁴, 0.1].

[0064] The measurements are: $\begin{array}{cc}{y}^{*}=\left[\begin{array}{c}\mathrm{North}\mathrm{GPS}\mathrm{position}\\ \mathrm{East}\mathrm{GPS}\mathrm{position}\\ \mathrm{Down}\mathrm{GPS}\mathrm{position}\\ \mathrm{Height}\mathrm{map}\end{array}\right],\mathrm{and}\delta y=\left[\begin{array}{c}{\stackrel{.}{r}}_{\mathrm{GPS}}^{\mathrm{NED}}-r_{i-}^{\mathrm{NED}}\\ -\delta d_{i-}\end{array}\right]& \left(6\right)\end{array}$

[0065] where δd is the vehicle down position with respect to the map. The sensitivity matrix is $\begin{array}{cc}H=\left[\begin{array}{ccccccccc}1& 0& 0& |& 0& 0& 0& 0& 0\\ 0& 1& 0& |& 0& 0& 0& 0& 0\\ 0& 0& 1& |& 0& 0& 0& 0& 0\\ 0& 0& 1& |& 0& 0& 0& 0& -1\end{array}\right]& \left(7\right)\end{array}$

[0066] The noise matrix is $\begin{array}{cc}R=\left[\begin{array}{cc}\left[P_{\mathrm{GPS}}\right]& 0\\ 0& {\left(0.03\right)}^2\end{array}\right].& \left(8\right)\end{array}$

[0067] where P_GPSis the position covariance from GPS or an override of P_GPS=10⁺⁴*I₃is used

[0068] A survey approach and map representation were developed to provide both a height measurement and a lane departure and warning capability. The map is constructed to give the location of the GPS antenna when the passenger side, right wheel is on the right lane marker. The map frame is Along Track (a), Cross Track (c), and Down (d). The map is parameterized verses Along Track. The map frame has origin at R₀(meters), and is rotated with respect to NED frame by azimuth from North in degrees (θ). To make the map, the GPS position points are put into the map frame. A cubic spline interpolation is used to get a map at uniform spacing for Along Path. The spline derivative, dc/da, is used to calculate φ(a), local line direction in the map frame. The map is in column format of a, c, d, φ, with a being uniformly spaced (nominally 2 meters). Because spline interpolation is used to produce the map on a defined grid, multiple vehicle tracks can be combined (averaged) to make a merged map.

[0069] To evaluate the vehicle position with respect to the map, the fixed NED to map transformation matrix, T_MN=(θ)₃, must be computed. This transformation matrix is based on a single rotation about the z-axis. The vehicle position is calculated in the map frame by $\begin{array}{cc}\left(\begin{array}{c}a\\ c\\ d\end{array}\right)=r^{\mathrm{Map}}=T_{\mathrm{MN}}\left(r^{\mathrm{NED}}-R_0\right)& \left(9\right)\end{array}$

[0070] where r^NEDis the estimated position in the NED frame.

[0071] The map is interpolated using a spline to generate the values {overscore (c)} (a), {overscore (d)} (a), and {overscore (φ)} (a). The vehicle position with respect to the map or right hand lane marker (delta lane, δl, and delta down, δd) is evaluated by

[0072] δc=c−{overscore (c)}(a)δl=δc*cos({overscore (φ)}(a)) δd=d−{overscore (d)}(a)  (10)

[0073] The delta lane measurement is the only desired output of the real time system.

[0074] Wheel turns from ABS with azimuth and pitch from the electronic compass are integrated to obtain the new navigated position, δ{circumflex over (r)}_i−. Integration, and filter propagation and navigation aid updates occur whether there is a real-time DGPS measurement present or not. The distance traveled over the period of the update is computed by summing the incremental distances formed by products of wheel circumference (k), accumulated wheel scale factor estimate (1+{circumflex over (α)}(1)) and delta wheel counts (ΔWC). The wheel circumference is measured and variations due to temperature and pressure are accounted for.

[0075] The navigated position integration is performed using the following equations: $\begin{array}{cc}{d}_{j,j-1}=k*\left(1+\hat{\alpha }\left(1\right)\right)*\Delta {\mathrm{WC}}_{j,j-1}{\stackrel{\_}{\psi }}_j=\frac{1}{2}*\left({\psi }_{j-1}+{\psi }_j\right)+{\psi }_B+{\psi }_C+\hat{\alpha }\left(2\right){\stackrel{\_}{\theta }}_j=\frac{1}{2}*\left(1+\hat{\alpha }\left(4\right)\right)*\left({\theta }_{j-1}+{\theta }_j\right)+{\theta }_B+\hat{\alpha }\left(3\right)\Delta r_{j,j-1}=d_{j,j-1}*\left[\begin{array}{c}\cos {\stackrel{\_}{\theta }}_j*\cos {\stackrel{\_}{\psi }}_j\\ \cos {\stackrel{\_}{\theta }}_j*\sin {\stackrel{\_}{\psi }}_j\\ -\sin {\stackrel{\_}{\theta }}_j\end{array}\right]r_j=r_{j-1}+\Delta r_{j,j-1}& \left(11\right)\end{array}$

[0076] Then the following are computed at each update interval from the loop quantities above: $\begin{array}{cc}{\stackrel{.}{d}}_i=\sum _{j=1}^{m=5}\left(d_{j,j-1}\right)/dt{\hat{r}}_{i-}={\hat{r}}_{\left(i-1\right)+}\sum _{j=1}^{m=5}\Delta r_{j,j-1}{\stackrel{\_}{v}}_i=\left({\hat{r}}_{i-}-{\hat{r}}_{\left(i-1\right)+}\right)/dt\stackrel{\_}{\psi }=\sum _{j=0}^{m=5}{\psi }_j/\left(m+1\right)\stackrel{\_}{\theta }=\sum _{j=0}^{m=5}{\theta }_j/\left(m+1\right),& \left(12\right)\end{array}$

[0077] where ψ=compass (magnetic) heading

[0078] θ=pitch (positive nose up)

[0079] WC=wheel counts or turns (fractional)

[0080] ψ_B=prior heading bias (“magnetic true”, sensor mounting and sensor bias)

[0081] ψ_C(ψ)=calibration foe vehicle magnetic properties (deviation)

[0082] θ_B=prior pitch bias (sensor mounting and sensor bias)

[0083] k=wheel circumference

[0084] Experimental results for the system of the present invention are shown in FIG. 6. The graph shows the output of the Kalman filter processor and lane deviations from the map during the drive. The test run also involved intentional lane departures, shown after 120 seconds on the graph.

[0085] Although the embodiments of the present invention have been described in detail, it will be understood that the present invention is not limited to the above-described embodiments, and various modifications in design may be made without departing from the spirit and scope of the invention defined in claims.