Position and velocity estimation system for adaptive weighting of GPS and dead-reckoning information

An embodiment of the present invention is a combined GPS and dead-reckoning (DR) navigation sensor for a vehicle in which a pair of modifications are made to an otherwise conventional Kalman filter. Process noise is adapted to cope with scale factor errors associated with odometer and turning rate sensors, and correlated measurement error processing is added. When only two Doppler measurements (PRRs), or three with an awkward three-satellite geometry, are available, DR error growth can nevertheless be controlled. The measurement error correlations in the conventional Kalman filter covariance propagation and update equations are explicitly accounted for. Errors induced by selective availability periods are minimized by these two modifications.

BACKGROUND OF THE INVENTION 
1. Field of the Invention 
The invention relates generally to navigation systems and more specifically 
to global positioning system (GPS) devices that use dead-reckoning 
apparatus to fill in as backup during periods of GPS shadowing such as 
occur amongst obstacles, e.g., tall buildings in large cities. 
Global positioning system receivers typically use signals received from 
three or more overhead satellites to determine navigational data such as 
position and velocity, and such systems may also provide altitude and 
time. GPS signals are available worldwide at no cost and can be used to 
determine the location of a vehicle, such as a car or truck, to within one 
city block, or better. Dual-frequency carrier GPS receivers typically 
track a pair of radio carriers, L1 and L2, associated with the GPS 
satellites to generate pseudo range measurements (PR) from precision code 
(P-code) or coarse acquisition code (C/A-code) modulation on those 
carriers. Carrier L1 is allocated to 1575.42 MHz and carrier L2 is 
positioned at 1227.78 MHz. Less expensive receivers tune to only one 
carrier frequency, and therefore cannot directly observe the local 
ionospheric delays that contribute to position error. At such frequencies, 
radio carrier signals travel by line-of-sight, thus buildings, mountains 
and the horizon can block reception. 
The constellation of GPS satellites in orbit about the earth comprises 
individual satellites that each transmit a unique identifying code in a 
code multiple access arrangement. This allows the many GPS satellites to 
all transmit in spread spectrum mode at the same frequency (plus or minus 
a Doppler shift from that frequency as results from the satellite's 
relative velocity). Particular satellites are sorted out of a resulting 
jumble of signals and noise by correlating a 1023 "chip" code to a set of 
predefined codes that are preassigned to individual GPS satellites. These 
codes do not arrive in phase with one another at the receiver. Therefore, 
"finding" a GPS satellite initially involves searching various carrier 
frequencies, to account for Doppler shift and oscillator inaccuracies, and 
searching for a code match, using 1023 different code phases and up to 
twenty-four possible correlation code templates. 
In large cities with many tall buildings, one or more of the GPS satellites 
that a particular receiver may be tracking may be temporarily blocked. In 
some situations, such blockage can prevent all the overhead GPS satellites 
from being tracked and such outages can last for several minutes. GPS 
signals also become unavailable to vehicles when moving through 
underground or underwater tunnels. Therefore a method and apparatus are 
needed to bridge an information gap that exists between periods of GPS 
signal availability. Dead-reckoning techniques have been used in the 
background art to supply navigation data, both alone and in concert with 
GPS systems. 
The prior art has not recognized nor taken full advantage of the fact that 
while within the typical "urban canyon," at least two GPS satellites are 
typically visible at any one instant. A significant performance advantage 
is possible if such GPS satellites are productively used to blend partial 
GPS information with dead-reckoning information. Such blending reduces the 
drift that is inherent in dead-reckoning. More accurate information is 
thus available on average, and overall accuracy can be maintained for 
relatively longer periods of GPS signal shadowing. 
A navigation system for a vehicle using a dead-reckoning system can 
encounter several sources of error. Initial position errors can result 
from GPS inaccuracies, especially in selective availability (SA) and 
multipath signal environments. A heading error may result from a 
difference between a vehicle's change in direction and the sensed change 
in direction, for example, as derived from a single-degree of freedom 
inertial gyro. Such errors can range from one to five percent for low-cost 
gyros. Heading errors can also stem from gyro rate bias/drift, scale 
factor non-linearity and initial warm-up problems. An odometer error is 
created by differences between the distance a vehicle actually travels and 
the vehicle's odometer indicated distance. Such errors can be classified 
as scale factor and scale factor non-linearity. Sensor measurement noise 
will also corrupt data obtained. Terrain sloping can cause a third type of 
error in that the ground traveled by a vehicle may exceed the horizontal 
distance traversed due to a change in altitude. 
SUMMARY OF THE PRESENT INVENTION 
It is therefore an object of the present invention to provide a system and 
method for using both partial and full GPS information in a blend of 
adaptively-weighted GPS and dead-reckoning determinations to provide 
vehicle position and velocity information to remote users. 
An object of the present invention is to provide a system and method for 
blending position and velocity solutions that allow for a filtering of 
errors between DR and GPS determinations in an intelligent way that 
recognizes the characteristics of the respective error sources. 
Briefly, a preferred embodiment of the present invention is a vehicle 
position and velocity estimation system that blends GPS solutions and 
dead-reckoning using a modified set of decoupled (decentralized) Kalman 
filters. Process noise is adapted to cope with scale factor errors 
associated with the odometer and turning rate sensor, and correlated 
measurement error modeling is added to a conventional Kalman filter. When 
only two Doppler measurements (PRRs), or three with an awkward 
three-satellite geometry, are available, an accumulated dead reckoning 
error can nevertheless be controlled. The correlated measurement error are 
explicitly accounted for in the modified Kalman filter covariance 
propagation and update computations. 
An advantage of the present invention is that it provides a system for 
continuous calibration of dead-reckoning sensor input data to provide 
accurate position solutions. 
Another advantage of the present invention is that a system is provided 
that automatically switches to a calibrated dead-reckoning solution when 
GPS satellite signals are blocked for any reason. 
Another advantage of the present invention is that a system is provided 
that can cause position, time and/or velocity to be automatically reported 
when an alarm sensor is activated. 
An advantage of the present invention is that a system is provided that can 
derive information from available GPS satellite signals when the number of 
GPS satellites visible is less than three. 
Another advantage of the present invention is that a system is provided 
that can be mounted within a vehicle and used to report a velocity and/or 
position of the vehicle even though GPS satellites may not always be 
visible at the vehicle's position. 
A further advantage of the present invention is that a system is provided 
in which the blending filters adapt to detected anomalies in both the GPS 
and DR sensor data, thereby improving a vehicle position and velocity 
estimation system response to anomalous performance of its sensors.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT 
FIG. 1 illustrates a global positioning system (GPS) mobile system 
embodiment of the present invention, referred to herein by the general 
reference numeral 10. System 10 has a six-channel vehicle tracking 
capability that uses GPS satellite signals to provide position and 
velocity information related to a vehicle 12 onboard which system 10 is 
mounted. System 10 comprises an odometer signal connection 14, a 
multi-channel GPS receiver 16, a GPS antenna 18 and a low-noise 
preamplifier 20. A signal from a turning rate sensor gyro 22 is input to a 
microcomputer system 24 for dead reckoning heading information input. Gyro 
22 is preferably a low-cost type, such as those based on piezoelectric 
solid state elements. Antenna 18 is preferably a hard mount patch antenna 
for permanent mounting. For example, good results have been obtained with 
antennas having dimensions of approximately 3.75 inches by four inches by 
0.75 inches, and by using a flange mount patch antenna for temporary 
mounting with dimensions in the order of 4.75 inches by four inches by 
1.75 inches. Antenna 18 is best placed on top of vehicle 12 where a view 
of GPS satellites will be the least obstructed by vehicle 12. 
A conventional Kalman filter when used to integrate GPS and dead reckoning 
information would attempt to represent the effects of selective 
availability (SA) which is the dominant GPS error source as white noise, 
or augment its state vector to include states which represent SA. Neither 
of these approaches would be appropriate in the present application: the 
effects of SA are highly correlated over minutes, so frequent GPS updating 
(e.g., once per second) would result in optimism in the Kalman filter 
covariance and resultant measurement under-weighting and perhaps rejection 
(simulation results will be presented later to illustrate this point). On 
the other hand, direct inclusion of SA error states leads to an 
unnecessary computational burden, and would jeopardize the ability to 
implement the design on a low-cost microprocessor (e.g., a 10 MHz Motorola 
68000 without math coprocessor). In addition, there are model fidelity 
issues relating to the inclusion of SA error states, i.e., how well SA can 
be modeled. Use of the approach described herein avoids these problems by 
representing the effects of SA in the Kalman update equations, without 
attempting to estimate SA directly. An efficient and robust design 
results, described in detail later in this application. 
GPS receiver 16 is such that it computes a complete vehicle 
position/velocity solution from signals received from at least three GPS 
satellites and it computes pseudo ranges (PRs) and pseudo range rates 
(PRRs) from signals received from GPS satellites tracked by the receiver. 
The GPS solutions or PRs and PRRs are used with data obtained from gyro 22 
and odometer signal 14 in a set of blending filters included as program 
software within the microcomputer system 24. A set of serial interfaces 
26-29 may be provided by microcomputer system 24. Specifically, interface 
26 may be a serial port suited for use with the Trimble Navigation 
(Sunnyvale, Calif.) ASCII interface protocol (TAIP). A modem 30 allows a 
radio 31 to transmit speed and location information regarding vehicle 12 
through an antenna 32. Such information would, for example, be useful to a 
central dispatcher of delivery trucks or of public safety vehicles, such 
as police and fire units. 
Microcomputer system 24 includes the basic dead reckoning equations, gyro 
and odometer interface conditioning and calibration, and filters 34, in 
computer firmware program form, for estimating a heading and velocity of 
vehicle 12, and for estimating how much error exists in the dead-reckoned 
position and in the position solutions derived from GPS receiver 16. A 
best estimate and estimation error variance are obtained. Errors 
associated with dead reckoning inputs are systematic in nature and 
therefore include components that can be estimated to yield more precise 
results. Therefore, while Kalman filter 34 is in large part conventional, 
it comprises modifications to prevent it from excluding heretofore 
unrecognized systematic errors as being simply white noise. For example, 
the heading variances produced are constrained to be relatively more 
conservative. 
FIG. 2 illustrates a GPS/DR navigation system 40 with a decentralized 
filter approach. A GPS receiver 42 provides pseudo ranges (PRs) and pseudo 
range rates (PRRs) to a GPS navigation computer 44, such as microcomputer 
24 (FIG. 1). A GPS velocity solution (V.sub.GPS) is fed to a heading 
calculator 46 that produces a GPS heading solution (H.sub.GPS). A heading 
filter 48 accepts inputs H.sub.GPS and a signal .DELTA.H.sub.C from the 
gyro 22 following the operation of a gyro scale factor calibrator 50 and a 
gyro bias calibrator 52. A vibrational gyro 54 provides a heading change 
signal (.DELTA.H). An odometer 56 provides a distance change signal 
(.DELTA.d) to both the gyro bias calibrator 52 and an odometer calibration 
filter 58. A velocity estimate (V) and a heading estimate (H) are received 
by a dead reckoning (DR) calculator 60. A position filter 62 receives both 
a DR change of position signal (.DELTA.p) and a GPS position solution 
(p.sub.GPS). A blended signal (L, .lambda.) is obtained by correcting the 
dead reckoning position and is output from a blender 64. 
FIG. 2 shows the interaction between the GPS receiver computed positions 
and velocities, the gyro and odometer data, and the three blending 
filters. The decentralized nature of the blending filters is in contrast 
with a more conventional, Kalman filter approach which collects all 
variables of interest in a single (larger) state vector. A conventional 
Kalman filter implementation is impractical in a low cost microprocessor, 
and therefore not preferred. Such a filter, however, may be partitioned 
into the three smaller filters: a heading filter, speed filter and a 
position filter. A significant reduction in computational requirements is 
possible, with no significant adverse impact on performance. 
Speed and heading information is derived from the GPS velocity computed by 
the GPS navigation computer 44. This information is input to the odometer 
calibration and heading filters 58 and 48. Each of these filters is a 
single state, Kalman-like filter, modified to account for SA-induced 
error, which is the dominant correlated measurement error contributor. The 
odometer scale factor calibration filter 58 examines the difference 
between odometer-derived velocity and GPS-derived velocity, and corrects 
the odometer scale factor, thereby improving the estimate of distance 
traveled input to the dead reckoning calculations. The gyro scale factor 
error is preferably calibrated at installation and periodically at 
startup, and may also be calibrated using GPS. The gyro bias is 
continuously updated using a separate filter, based upon the knowledge 
that the vehicle 12 is stationary, as determined from the odometer 56. The 
gyro bias calibrator (filter) 52 preferably includes a bias monitor, which 
examines the stability of the calculated bias, and makes adjustments to 
the parameters of the heading filter based upon the predicted stability. 
The gyro sensed heading rate, compensated for its bias, along with a 
measure of the bias stability, is input to the heading filter 48, where it 
is blended with GPS-derived heading to improve the heading which is passed 
to the dead reckoning equations. The outputs of the dead reckoning 
calculations (e.g., the predicted latitude and longitude of the vehicle 
12), are input to the position filter, which generates corrections to them 
based upon the GPS-derived positions. The position filter is also 
implemented as a Kalman filter modified to handle correlated errors which 
are dominated by SA. The corrected latitude and longitude, appropriately 
time-tagged, is then passed to the modem/radio combination for 
communication to the outside world. 
Microcomputer system 24 prevents position and velocity solutions that are 
abruptly deviant from previous solutions from being fully weighed into 
final position and velocity solutions. In the prior art, it is not 
uncommon for GPS-derived solutions to "jump" from one reading to the next. 
GPS position solutions can change abruptly, e.g., due to constellation 
changes, which are especially significant when operating 
non-differentially. 
In particular, GPS position solutions can appear to be discontinuous 
("jump") whenever reflected signals and/or radio frequency interference is 
present. Such conditions are common in the so-called "urban canyon" 
environment. It is however, characteristic of DR solutions to not Jump. 
Therefore, Kalman filter 34 includes modifications to a conventional 
Kalman filter that exclude or de-emphasize (de-weight) GPS solutions that 
have jumped by magnitudes that the DR solutions tend to indicate as being 
impossible or improbable. 
However, an override mechanism is needed to allow an ever increasing body 
of GPS-derived solutions to overcome what first appeared to be an 
impossible departure from the DR solutions. Such a situation could occur 
if vehicle 12 was transported a substantial distance by ferry, in which 
case the indicated travel distance at odometer input signal 14 would be 
near zero. 
In general, Kalman filters cannot be applied without recognizing the 
effects of correlated error, dominated by SA, which is applied by the 
Department of Defense. The effects of SA cannot be filtered out over time, 
because the effects of SA are intentionally not equivalent to white noise. 
Therefore, Kalman filter 34 accommodates SA by making a special 
modification to a conventional Kalman filter algorithm. An implicit 
modeling of SA is included in Kalman filter 34 such that when the Kalman 
filter 34 does an update the decay is forced to assume a rate that has 
been empirically determined to be consistent with previous observations of 
SA, instead of allowing a decay as 1/.sqroot.n, where "n" is the number of 
uncorrelated measurements of a stationary observable. For example, SA will 
vary in its effect very little from second to second, but over a period of 
a minute to a hundred seconds, SA can decorrelate. For a standard 
deviation of thirty meters and ten measurements during stationary 
operation, for example, conventional Kalman filters will predict an error 
standard deviation of roughly ten meters. This error variance will be 
grossly optimistic if the ten measurements are processed over a time 
interval which is short, relative to the expected SA period. If the 
measurements were generated over a period of ten seconds, for example, the 
error variance of the conventional Kalman filter would be optimistic by 
roughly a factor of ten. This could lead to de-weighting or rejection of 
subsequent GPS measurements and divergence of the resulting position 
solution. 
FIG. 3 illustrates a Kalman filter 70 partitioned into a set of coupled 
filter units, including an enhanced heading filter 72, a velocity (speed) 
filter 74 and a position filter 76. In an alternative embodiment of the 
present invention, the enhanced heading and speed filters 72 and 74 may be 
substituted with a preferred heading and speed filter 78, and a preferred 
position filter 80. Heading filter 78 is preferred in instances where the 
interface with GPS receivers 16 or 42 (FIGS. 1 or 2) permits access to 
individual GPS measurements, e.g., when only two PRRs are available 
because a third GPS satellite is unavailable or its signal is being 
blocked for some reason. Heading sand speed filters 72 and 74 are used 
when the interface, e.g., with GPS receiver 16, is limited to access of 
only GPS-based velocity, as might be the case for commercially available 
GPS receivers. Similarly, the enhanced position filter 76 is limited to 
use of GPS-derived positions, whereas the preferred position filter can 
make use of individual pseudo range measurements. Nevertheless, filters 
72, 74 and 76 combine to provide modeling of the adverse effects of 
correlated error, e.g., SA, and scale factor errors, e.g., from gyro 22 
and odometer input signal 14. A level of adaptability to gyro and/or 
odometer failures is desirable and included in Kalman filter 70. 
The enhanced heading filter 72 is preferably a computer-implemented process 
for execution by microcomputer 24, and is described mathematically by 
equation (1), as follows: 
given: GPS-derived heading from a velocity solution 
include minimum velocity qualification testing 
estimate accuracy of GPS-derived heading: 
##EQU1## 
where: HDOP=horizontal dilution of precision value output with a current 
fix, 
.sigma..sub.asa =one-sigma SA acceleration magnitude, 
.sigma..sub.vsa =one-sigma SA velocity error, 
v=current speed from GPS receiver 16, 
and 
.DELTA.t.sub.c =age of differential correction in seconds. 
Typical values for .sigma..sub.asa are 0.005-0.01 meters per second 
squared, while typical values for .sigma..sub.vsa include 0.25-0.50 meters 
per second. A constant value for .DELTA.t.sub.c may be assumed, since this 
information may not be readily available in real-time for the enhanced 
filters. Equation (1) specifies the error variance of the GPS-derived 
heading, which is assumed to include a temporally correlated component, 
dominated by SA, and a white-noise component .sigma..sub.v. Equation (1) 
applies to a three-dimensional velocity fix, e.g., from GPS receiver 16. A 
more conservative calculation is necessary if using a two-dimensional 
velocity fix. Modeling such a correlated error component prevents the 
Kalman filter 70 from becoming unrealistically optimistic. The heading 
error correlation is represented by .sigma..sub.c, wherein: 
EQU initialization: .sigma..sub.c =0 
EQU propagation: .sigma..sub.c.sbsb.k+1.sup.-=.PHI..sub.sa 
.sigma..sub.c.sbsb.k.sup.+ (2) 
EQU update: .sigma..sub.c.sbsb.k+1.sup.+=(1-k).sigma..sub.c.sbsb.k+1.sup.- +k 
.sigma..sub.H/GPS/C.sup.2 (3) 
where: 
k=pseudo-Kalman measurement gain 
.sigma..sup.2.sub.H/GPS/C =first term in equation (1) excluding 
.sigma..sub.v. 
##EQU2## 
Equation (2) is used to propagate the heading error correlation in time 
(e.g., in between GPS updates), while equation (3) indicates how the 
correlation changes across a measurement update. 
Typical values for .tau..sub.sa are 60-200 seconds. A decay of the SA 
correlation is assumed to be linear in the propagation interval. This 
represents an approximation to the exponential decorrelation which is 
valid for time periods that are short, relative to the decorrelation time 
associated with SA. This represents a very good approximation most of the 
time, and saves the computation of the exponential term. When the 
approximation breaks down, e.g., when the time interval approaches the 
decorrelation time, the correlation is simply set to zero. If the 
constellation on which the velocity solution is based changes 
significantly, it may be worthwhile to also set .PHI..sub.sa =0. 
For heading update: 
EQU H.sub.k+1.sub.- =H.sub.k.sup.+ +.DELTA.H.sub.gyro, (4) 
where: 
.DELTA.H.sub.gyro has been compensated for bias, 
EQU H.sub.res =H.sub.GPS.sbsb.k+1 -H.sub.k+1.sup.- 
where: 
EQU H.sub.GPS =GPS-derived heading. 
For a statistical reasonableness test on H.sub.res : 
EQU compute .sigma..sub.res.sup.2 =.sigma..sub.H.sbsb.k+1.sup.2 -2 
.sigma..sub.c.sbsb.k+1 +.sigma..sub.H/GPS.sup.2. (b 5) 
Two parameters control the test (C.sub.1, C.sub.2), 
EQU if H.sub.res.sup.2 .gtoreq.C.sub.1 .sigma..sub.res.sup.2 and 
H.sub.res.sup.2 &lt;C.sub.2 .sigma..sub.res.sup.2, (6) 
reduce pseudo Kalman gain: k&lt;-k/C.sub.2 
EQU if H.sub.res.sup.2 .gtoreq.C.sub.2 .sigma..sub.res.sup.2 
bypass measurement update. 
Tentatively, C.sub.1 =9, C.sub.2 =25 (alternatively, C.sub.2 =C.sub.1). The 
residual tests involving the parameters C.sub.1 and C.sub.2 are intended 
to reduce the solution sensitivity to unmodelled errors associated with 
the GPS-derived heading. Two such error mechanisms are the generation of a 
solution based upon either reflected signals and/or signals "spoofed" by 
RF interference. 
EQU k=(.sigma..sub.H.sbsb.k+1.sup.2 -.sigma..sub.c)/.sigma..sub.res.sup.2, (7) 
EQU H.sub.k+1.sup.+ =H.sub.k+1.sup.- +k H.sub.res. (8) 
There is a difference in form for the "Kalman gain" k appearing in equation 
(7) compared to the conventional gain calculation: the correlation term is 
subtracted from the a priori error variance, and also used to adjust the 
residual variance (equation 4). The effect of the correlation term, 
representing the effects of SA, is to increase the gain floor of the 
filter, and keep the filter "aware" of the effects of the correlation, 
e.g., to prevent it from "falling asleep". These effects are illustrated 
in FIGS. 3 and 4, which compare the results of the modified filter with a 
conventional Kalman filter. The plots are derived from a Monte Carlo 
simulation of both filter designs run on a IBM-compatible 386/25 MHz 
personal computer, with realistic models for the gyro, odometer and GPS. 
They correspond to a vehicle initially traveling in a straight line at 
forty miles per hour (MPH) for roughly one hundred seconds, and then 
slowing to twenty MPH. GPS velocity updates are assumed to be available 
each second, and differential corrections are not available to the 
vehicle. Thus the impact of SA on navigation error is maximum. 
FIG. 4 plots the gain history for a conventional filter, followed by its 
error and covariance histories in FIG. 5. 
FIGS. 6 and 7 represent the corresponding plots for the modified filter 
design of FIG. 3. The differences in gain and covariance behavior are such 
that the conventional filter is more optimistic and uses gains which are 
too low. These would typically begin rejecting measurements if such 
testing was included in the simulation, making the improvements even more 
dramatic. In fact, as evidenced by FIG. 5, the conventional filter assumes 
it has averaged out SA error, and predicts a one-sigma error of roughly 
one-half degree, when more than three degrees of error are actually 
present. In contrast, a significant improvement is realized with the 
modified Kalman filter. During the first one hundred seconds of the 
simulation, its expectation of roughly 1.5 degrees one sigma, are more 
consistent with the SA error. Following the speed change, the heading 
error is substantially reduced to roughly 0.2 degrees. Such a reduction is 
possible by the removal of the SA contribution, and there is a negative 
gain at the transition. Thus in the simulated run, more than a factor of 
ten improvement is realized with the modified filter. 
For covariance propagation, .sigma..sub.c propagation is described by 
equation (2). 
In modeling the effects of gyro scale factor error, it is assumed that 
residual errors for clockwise (CW) and counterclockwise (CCW) rotations of 
vehicle 12 are statistically uncorrelated. There are, therefore, separate 
specifications, .sigma..sub.SF+.sup.2 and .sigma..sub.SF-.sup.2. These 
specifications may be adapted as a function of the changes in the 
calculated scale factor factors, if they are periodically or dynamically 
computed. However, .sigma..sub.SF+.sup.2 and .sigma..sub.SF.sup.2 -, are 
assigned the same initial values. 
Two separate sums, .DELTA.H.sub.gryo+sum, and .DELTA.H.sub.gryo-sum are 
computed and correspond to the different signs of the heading changes, as 
illustrated by the following pseudocode: 
##EQU3## 
In the propagation sums, the terms .DELTA.H.sub.gryo+sum.sup.s, 
.DELTA.H.sub.gryo-sum, .DELTA.H.sub.gryo+sum.sup.s, .DELTA.H.sub.gryo-sum 
and q.sub.SF are all preferably initialized to zero. The term 
q.sub..DELTA.H is time varying. It is adapted as a function of the 
stability of the turning rate sensor gyro 22 bias as determined by its 
calibration history. 
For covariance update equations, .sigma..sub.c is updated after a 
.sigma..sub.H.sup.2 update, and is described by equation (3), and 
EQU .sigma..sub.H.sbsb.k+1.sup.2+ =(1-k).sup.2 .sigma..sub.H.sbsb.k+1 +2.sup.2- 
k(1-k) .sigma..sub.c.sbsb.k+1.sup.- +k.sup.2 .sigma..sub.H/GPS.sup.2. (10) 
Tests are done on zeroing out heading (H) sums used in a propagation: 
##EQU4## 
This special handling of the effects of the gyro scale factor error 
represents a second distinction of the present invention relative to a 
conventional Kalman filter. This error is not well represented by white 
noise, since it is bias-like. These special summations are therefore 
required to force the filter to maintain conservatism, with respect to the 
heading error induced by gyro scale factor error. In order to avoid 
injecting too much conservatism, tests must be performed based upon the 
updated covariance (controlled by the parameter fraction), to reset the 
summations formed during the turn. An additional term is added to the GPS 
heading variance, equation (1), if the velocity solution is based on a 
propagated measurement. 
The velocity (speed) filter 74 is implemented as a single-state, 
Kalman-like filter for estimating an error associated with an odometer 
scale factor from a GPS-derived velocity. A basic Kalman filter equations 
are adapted to improve correlated error component handling. Correlated 
velocity error develops either directly when operating non-differentially, 
or through leakage in differential corrections. Scale factor error is 
modeled as a Markov process with a fairly long time constant, e.g., 
300-900 seconds. This time constant reflects a plurality of variations, 
e.g., those influences attributable to the effects on wheel radius due to 
tire pressure changes. Thus, this is a relatively slowly varying effect. 
Higher frequency variations in the scale factor are averaged out. 
For example: 
EQU let .delta.v.sub.SF =odometer scale factor error state, 
EQU .sigma..sub.c =error correlation induced by SA. 
It is assumed that .delta.v.sub.SF is subtracted from the current odometer 
scale factor at each measurement update, e.g., as frequently as one Hz. It 
is therefore not necessary to propagate an error estimate. Letting 
V.sub.GPS =GPS-derived speed in meters per second, a measurement is formed 
as the difference, 
EQU Z=V.sub.o V.sub.GPS (12) 
where: 
V.sub.o =odometer derived velocity. 
The measurement is therefore, 
EQU Z=V .delta.v.sub.SF -.delta.v.sub.GPS, 
where, V=true velocity. The measurement gradient "vector" is thus V, well 
approximated by V.sub.o. 
There is the possibility of a significant latency error between V.sub.o and 
V.sub.GPS, which is preferably not neglected. The latency may be as much 
as one second, and can produce a significant impact, e.g., whenever 
vehicle 12 accelerations and decelerations are substantial. Therefore, a 
computed measurement noise variance is preferably increased by an 
estimated acceleration level, and is based on backward differencing 
odometer velocities, 
EQU a=(Vo.sub.k -Vo.sub.k-1)/ .DELTA.t.fwdarw.a.DELTA.t=Vo.sub.k -Vo.sub.k-1. 
(13) 
Computing measurement noise variances, 
##EQU5## 
where: .sigma..sub.vsa =one-sigma PRR error from SA, 
.sigma..sub.asa =one-sigma PRR error from SA, 
HDOP=Horizontal Dilution of Precision associated with a current velocity 
fix, 
and, 
.DELTA.t.sub.c =average differential latency in seconds. 
In dividing a.DELTA.t by four, the maximum error corresponding to a one 
second latency is identified as a two-sigma value. The HDOP may be 
increased when a solution is two-dimensional, owing to the use of a 
possibly incorrect, fixed altitude, as in equation (1). 
Since the estimated scale factor error is applied to a whole valued 
estimate at each measurement update. It is not necessary to propagate 
.delta.v.sub.SF. However, the correlation term, .sigma..sub.c and 
.sigma..sub.SF.sup.2 are propagated, where .sigma..sub.c is initialized to 
zero. 
Computing a state transition factor for SA-: 
##EQU6## 
where: .tau.=a time constant associated with odometer scale factor 
(typically 300-900 seconds), 
and, 
.sigma..sub.VSF.sbsb.SS.sup.2 =a steady-state error variance associated 
with the odometer scale factor error. 
The value of .sigma..sub.VSF.sbsb.O.sup.2 need not be set to 
.sigma..sub.VSF.sbsb.SS.sup.2, especially if large initial uncertainty 
exists. 
For "Kalman" gain computations, 
EQU k=(V.sub.o .sigma..sub.SF.sup.2 +.sigma..sub.c)/ (V.sub.o.sup.2 
.sigma..sub.SF.sup.2 +2V.sub.o .sigma..sub.c +.sigma..sub.VGPS.sup.2). 
(17) 
For measurement updates, 
Residual test: (parameters are C.sub.1 and C.sub.2), and 
EQU Resvar=V.sub.o.sup.2 .sigma..sub.SF.sup.2 +2V.sub.o .sigma..sub.c 
+.sigma..sub.VGPS.sup.2 (18) 
If (Z.sup.2 /Resvar)&gt;C.sub.1 k/C.sub.1, 
If (Z.sup.2 /Resvar)&gt;C.sub.2 bypass update; 
Covariance and correlation update: 
EQU .sigma..sub.SF.sup.2 =(1-kv).sup.2 .sigma..sub.SF.sup.2 
-2k(1-kv).sigma..sub.c +k.sup.2 .sigma..sub.VGPS.sup.2, and (19) 
EQU .sigma..sub.c =(1-kv).sigma..sub.c -k.sigma..sub.CVGPS.sup.2, (20) 
where .sigma..sub.CVGPS.sup.2 represents the correlated component of the 
measurement error. An additional term is added to the computed measurement 
noise variance if the velocity solution is based on propagated 
measurements. 
The position filter 76 uses corrected outputs of the heading and speed 
filters 72 and 74, and processes GPS receiver 16 position fixes. Two and 
three-dimensional and propagated solutions are included, albeit weighted 
differently. Kalman filter 76 is a two-state type that accounts for GPS 
receiver 16 error correlation due to SA and is used to process the data. 
The propagation equations for Kalman filter 76 begin with a construction 
of an average heading and velocity, using outputs from the heading and 
speed filters 72 and 74: 
EQU H.sub.avg =(H.sub.k +H.sub.k-1)/2, and (21) 
EQU V.sub.avg =(V.sub.k +V.sub.k-1)/2, (22) 
where the k subscript denotes "current" time, e.g., the time of the GPS 
receiver 16 position fix, or the time to which position is being 
propagated. Associated with these average values are the following average 
variances: 
EQU .sigma..sub.H.sbsb.avg.sup.2 =(.sigma..sub.H.sbsb.k.sup.2 
+.sigma..sub.h.sbsb.k-1.sup.2 +2 max (.sigma..sub.H.sbsb.k.sup.2, 
.sigma..sub.H.sbsb.k-1.sup.2))/4; and (23) 
EQU .sigma..sub.V.sbsb.avg.sup.2 =(.sigma..sub.V.sbsb.k.sup.2 
+.sigma..sub.V.sbsb.k-1.sup.2 +2 max (.sigma..sub.V.sbsb.k.sup.2, 
.sigma..sub.V.sbsb.k-1.sup.2))/4 (24) 
where .sigma..sub.V.sup.2 =V.sup.2 .sigma.hd SF.sup.2. In both equations 
(23) and (24), an approximation to the correlation between successive 
values of heading and velocity error, respectively, is made. A correlation 
coefficient of one is assumed. The product of the standard deviations by 
the maximum variance is approximated, to avoid computing a square root. 
This is a conservative approximation. 
The variances associated with east and north position error are propagated 
using equations (25) through (29). 
EQU .sigma..sub..DELTA.Pe.sup.2 +=.DELTA.d.sub.sum.sup.s (sin.sup.2 H.sub.avg 
.sigma..sub.Havg.sup.2 +cos.sup.2 H.sub.avg .sigma..sub.VSF.sup.2); (25) 
EQU .sigma..sub..DELTA.Pn +=.DELTA.d.sub.sum.sup.s (cos.sup.2 H.sub.avg 
.sigma..sub.Havg.sup.2 +sin.sup.2 H.sub.avg .sigma..sub.VSF.sup.2); (26) 
EQU .DELTA..sigma..sub.sum.sup.2 +=.DELTA.d.sub.sum.sup.s 
(.sigma..sub.Havg.sup.2 +.sigma..sub.VSF.sup.2); and (27) 
EQU .sigma..sub..DELTA.Pe.DELTA.Pn.sup.2 +=sin H.sub.avg .DELTA.d.sub.sum.sup.s 
(.sigma..sub.VSF.sup.2 -.sigma..sub.Havg.sup.2) (28) 
where, 
EQU .DELTA.d.sub.sum.sup.2 =.DELTA.d.sup.2 +2.DELTA.d.DELTA.d.sub.sum, (29) 
EQU .DELTA.d.sub.sum +=.DELTA.d. (30) 
The terms sin H.sub.avg and cos H.sub.avg are calculated previously as a 
part of the position propagation, and therefore need not be recalculated 
here. 
Equations (25) through (30) represent the covariance propagation equations, 
except for the correlated error correlation propagation. State propagation 
equations are: 
EQU L.sub.k =L.sub.k-1 +.DELTA.p.sub.n /R.sub.e (latitude in radians); and (31) 
EQU .lambda..sub.k =.lambda..sub.k-1 +.DELTA.p.sub.e /(R.sub.e cos L) (latitude 
in radians). (32) 
In equations (31) and (32), 
EQU .DELTA.p.sub.e =.DELTA.d sin H.sub.avg, and (33) 
EQU .DELTA.p.sub.n =.DELTA.d cos H.sub.avg, (34) 
where, 
.DELTA.d=V.sub.avg .DELTA.t. 
Measurement processing begins with the creation of east and north position 
measurement residuals: 
EQU .DELTA..sub.pe/res =(.lambda..sub.DR -.lambda..sub.GPS).multidot.R.sub.e 
cos L (in meters); and (35) 
EQU .DELTA..sub.pn/res =(L.sub.DR -L.sub.GPS).multidot.R.sub.e, (in meters) 
(36) 
where, .lambda..sub.GPS L.sub.GPS are the longitude and latitude of the 
current GPS receiver 16 fix in radians, respectively. 
The position filter 76 forms the following estimates of the east and north 
components of the DR position error: 
EQU .DELTA.pe=K.sub.ee .DELTA.p.sub.e/res +k.sub.en .DELTA.p.sub.n/res (in 
meters), and (37) 
EQU .DELTA.pn=K.sub.ne .DELTA.p.sub.e/res +k.sub.nn .DELTA.p.sub.n/res (in 
meters), (38) 
where, gains k.sub.en, k.sub.en, k.sub.en and k.sub.en are expressed in 
equations herein. Both residuals affect the east and north position error 
estimates, since the error components are correlated through the common 
heading and odometer errors. 
The error estimates computed by equations (37) and (38) are used to correct 
propagated DR positions: 
EQU L.sub.k =.DELTA.pn/R.sub.e ; and (39) 
EQU .lambda..sub.k =.DELTA.pe/(R.sub.e cos L) (40) 
The gain terms: k.sub.en, k.sub.ne, k.sub.ee and k.sub.nn, are computed by 
first forming intermediate variables: 
EQU .sigma..sub.ediff.sup.2 =.sigma..sub.e.sbsb.GPS.sup.2 -2.sigma..sub.ce 
+.sigma..sub..DELTA.pe.sup.2 ; (41) 
EQU .sigma..sub.ndiff.sup.2 =.sigma..sub.n.sbsb.GPS.sup.2 -2.sigma..sub.cn 
+.sigma..sub..DELTA.pn.sup.2 ; (42) 
EQU .sigma..sub.cdiff =.sigma..sub..DELTA.pe.DELTA.pn -.sigma..sub.c.sbsb.en 
-.sigma..sub.c.sbsb.ne ; (43) 
EQU .sigma..sub.cdiffe =.sigma..sub..DELTA.pe.DELTA.pn -.sigma..sub.c.sbsb.en ; 
(44) 
EQU .sigma..sub.cdiffn =.sigma..sub..DELTA.pe.DELTA.pn -.sigma..sub.c.sub.ne ; 
(45) 
EQU .sigma..sub.e.sbsb.unc.sup.2 =.sigma..sub..DELTA.pe.sup.2 
-.sigma..sub.c.sbsb.e ; and (46) 
EQU .sigma..sub.n.sbsb.unc.sup.2 =.sigma..sub..DELTA.pn.sup.2 
-.sigma..sub.c.sbsb.n (47) 
where: 
.sigma..sub..DELTA.pe.sup.2, .sigma..sub..DELTA.pn.sup.2 and 
.sigma..sub..DELTA.pe.DELTA.pn are propagated per equations (25) through 
(30), 
.sigma..sub.ce, .sigma..sub.cn, .sigma..sub.cne and .sigma..sub.cen are 
correlations which are propagated and updated and all initialized to zero, 
and 
.sigma..sub.e.sbsb.GPS.sup.2 and .sigma..sub.n.sbsb.GPS.sup.2 characterize 
the accuracy of a current fix. 
When characterizing the current fix, it is assumed that only HDOP is 
available. Additional information, including EDOP, NDOP, a correlation 
term and residual information from an over-determined solution is 
preferable, but not strictly necessary. With only HDOP, the north and east 
position error components of a fix are assumed to be of equal variance and 
uncorrelated, e.g., .sigma..sub.en =0. The variances are computed as 
follows: 
EQU .sigma..sub.e.sbsb.GPS.sup.2 =.sigma..sub.n.sbsb.GPS.sup.2 =HDOP.sup.2 
(.sigma..sub.cp/GPS.sup.2 +.sigma..sub.pn.sup.2), (48) 
where: 
.sigma..sub.pn.sup.2 =noise variance, about one meter square, 
.sigma..sub.cp/GPS.sup.2 =correlated error variance, which includes both 
multipath, correlation induced by the code carrier filter, and SA or 
SA-leakage, 
##EQU7## 
and 
EQU .sigma..sub.corr.sup.2 =(30 meters).sup.2. 
If UDRE is not available from GPS receiver 16, then a constant value for 
.sigma..sub.diff.sup.2 is assumed. 
For gain equations: 
EQU det=.sigma..sub.ediff.sup.2 .sigma..sub.ndiff.sup.2 
-.sigma..sub.cdiff.sup.2, (49) 
EQU k.sub.ee =(.sigma..sub.ndiff.sup.2 .sigma..sub.e.sbsb.unc.sup.2 
-.sigma..sub.cdiff .sigma..sub.cdiff.sbsb.n)/det, (50) 
EQU k.sub.en =(.sigma..sub.ediff.sup.2 .sigma..sub.cdiff.sbsb.n 
-.sigma..sub.cdiff .sigma..sub.e.sbsb.unc.sup.2)/det, (51) 
EQU k.sub.ne =(.sigma..sub.ndiff.sup.2 .sigma..sub.cdiffe.sbsb.e 
-.sigma..sub.cdiff .sigma..sub.n.sbsb.unc.sup.2)/det, and (52) 
EQU k.sub.nn =(.sigma..sub.ediff.sup.2 .sigma..sub.n.sbsb.unc.sup.2 
-.sigma..sub.cdiff .sigma..sub.cdiff.sbsb.e)/det. (53) 
For covariance and correlation update: 
##EQU8## 
where: .sigma..sub.nGPS/c.sup.2 =.sigma..sub.eGPS/c.sup.2 =HDOP.sup.2 
.sigma..sub.cp/GPS.sup.2. 
For the time propagations of the correlations: 
EQU .sigma..sub.cn =.PHI..sub.SA .sigma..sub.cn, (61) 
EQU .sigma..sub.ce =.PHI..sub.SA .sigma..sub.ce, (62) 
EQU .sigma..sub.c.sbsb.en =.PHI..sub.SA .sigma..sub.c.sbsb.en, and (63) 
EQU .sigma..sub.c.sbsb.ne =.PHI..sub.SA .sigma..sub.c.sbsb.ne. (64) 
The correlations in equations (61) through (64) represent the effects of 
SA, but other effects may be included, e.g., uncompensated ionospheric 
delay. 
If a position solution is based on propagated measurements, then additional 
terms which reflect the result of sensed acceleration on the computed 
position are added to the GPS receiver 16 measurement error variance. 
Both .DELTA.d.sub.sum and .DELTA..sigma..sub.psum.sup.2 are reset to zero 
whenever the updated variances .sigma..sub..DELTA.pe.sup.2 and 
.sigma..sub..DELTA.pn.sup.2 will pass the following test: 
##EQU9## 
A significant simplification to the preceding equations for the two-state 
position filter, e.g., equations (25)-(28) and (54)-(56), can be obtained 
if it is assumed that the measurement processing of the two filters can be 
decoupled. Equations (37) and (38) would thereby become, 
EQU .DELTA.pe=k.sub.ee .DELTA.p.sub.e/res, and (66) 
EQU .DELTA.pn=k.sub.nn .DELTA.p.sub.n/res. (67) 
This approximation basically neglects the correlation between east and 
north position error, and the two-state filter decouples into two 
single-state filters. There is also no longer a need to implement equation 
(28), since the correlation no longer appears in the measurement 
processing equations. Verification of the operation of the simplified 
design has indicated that the performance losses are not significant. It 
is therefore a reasonable alternative which results in substantial 
computational savings. The values for the gains in the two decoupled 
filters are given by, 
EQU k.sub.ee =k.sub.e =(.sigma..sup.2 .DELTA..sub.pe -.sigma..sub.ce)/ 
(.sigma..sup.2 .DELTA..sub.pe -2.sigma..sub.ce +.sigma..sub.eGPS.sup.2), 
and (68) 
EQU k.sub.nn =k.sub.n =(.sigma..sup.2 .DELTA..sub.pn -.sigma..sub.cn)/ 
(.sigma..sup.2 .DELTA..sub.pn -2.sigma..sub.cn +.sigma..sub.nGPS.sup.2). 
(69) 
The covariance update equations simplify to, 
EQU .sigma..sub..DELTA.pe.sup.2 =k.sub.e.sup.2 .sigma..sub.e.sbsb.GPS.sup.2 +2 
k.sub.e (1-k.sub.e).sigma..sub.c.sbsb.e +(1-k.sub.e).sup.2 
.sigma..sub..DELTA.pe.sup.2, and (70) 
EQU .sigma..sub..DELTA.pn.sup.2 =k.sub.n.sup.2 .sigma..sub.n.sbsb.GPS.sup.2 +2 
k.sub.n (1-k.sub.n).sigma..sub.c.sbsb.n +(1-k.sub.n).sup.2 
.sigma..sub..DELTA.pn.sup.2. (71) 
Therefore, two, rather than the previously required four, correlations are 
needed. Their propagation equations are, 
EQU .sigma..sub.c.sbsb.e =(1-k.sub.e).sigma..sub.c.sbsb.e +k.sub.e 
.sigma..sub.cp.sbsb.GPS.sup.2, and (72) 
EQU .sigma..sub.c.sbsb.n =(1-k.sub.n).sigma..sub.c.sbsb.n +k.sub.n 
.sigma..sub.cp.sbsb.GPS. (73) 
The preferred heading filter 78 combines individual GPS pseudo-range rate 
(PRR), or Doppler measurement, from GPS receiver 16, an odometer-derived 
linear velocity from input signal 14 and a gyro-derived heading rate, such 
as from gyro 22. A differential odometer may be used in place of gyro 22 
to sense heading changes. Differential odometers measure differences in 
the distances traversed by the left and right sides of vehicle 12. 
Preferred heading and speed filter 78 operates continuously, processing 
PRR measurements from individual GPS satellites. Since one or more GPS 
satellites are expected to be in view most of the time, a substantial 
performance advantage relative to the enhanced heading filter is probable 
during periods of dependency on dead-reckoning. 
Preferred heading and speed filter 78 is mechanized as a four-state Kalman 
filter of (1) gyro or differential odometer heading error, (2) odometer 
scale factor error, (3) terrain slope and (5) local clock frequency error. 
The first state includes a gyro drift component, the variance of which is 
proportional to time duration and error induced by residual scale factor 
error. The second state represents the residual scale factor error of the 
odometer itself, modeled as a relatively slowly varying Markov process. 
The third state models the local slope of the terrain as a spatially 
varying Markov process with standard deviation and correlation distance 
parameters set by a user as a function of the expected terrain roughness. 
The fourth state represents the error in the frequency standard of the GPS 
receiver. 
Two modifications are made to an otherwise conventional Kalman filter. 
Process noise is adapted to cope with scale factor errors, and correlated 
measurement error processing is added. The measurement error correlations 
in the conventional Kalman filter covariance propagation and update 
equations are explicitly accounted for. 
TABLE I 
______________________________________ 
Preferred Heading and Speed Filter 
State Definition Notation 
State 
Description State Model Use 
______________________________________ 
.delta.H 
estimated error 
zeroed out following 
can be used to 
in the incorporation of each 
refine the 
propagated gyro 
measurement after 
gyro or 
or differential 
propagated heading is 
differential 
odometer corrected odometer scale 
heading in factor 
radians 
.delta.S 
estimated local 
modeled as a Markov 
this state is 
terrain slope 
process, with an a 
included when 
priori error variance 
operating with 
and correlation 
a full set of 
distance computed 
satellites and 
from terrain roughness 
vertical 
specifications velocity is 
significant 
.delta.d.sub.SF 
estimate of the 
modeled as a Markov 
residual, process, with an a 
linear odometer 
priori error variance 
scale factor that reflects the 
error accuracy of an initial 
calibration and a time 
constant that reflects 
the variability of 
tire pressure with 
temperature, the 
shorter term 
variations expected 
due to vehicle 12 
velocity are thus 
averaged 
.delta.f 
estimate of modeled as a random 
local clock walk, with process 
frequency error 
noise selected to 
represent the 
frequency error 
stability 
______________________________________ 
All states except .delta.H are free-running, e.g., they are not normally 
set to zero following each measurement update. The odometer scale factor, 
if adjusted, should result in .delta.d.sub.SF being reset to zero. 
Only ten elements of a four-by-four covariance matrix, equation (66), are 
propagated and updated for maximum efficiency, as illustrated in equation 
(74), 
##EQU10## 
where "x" are redundant and ignored. 
In addition to the four basic filter states, there is a "consider" state 
that represents the effects of selective availability for non-differential 
operation or selective availability leakage for differential operation. 
This "consider" state implies that four additional correlations per 
satellite must be included in the covariance update and propagation 
equations. These correlations are labeled .sigma..sub.c.delta.H.sbsb.i, 
.sigma..sub.c.delta.S.sbsb.i, .sigma..sub.c.delta.d.sbsb.i, and 
.sigma..sub.c.delta.f.sbsb.i, where i=1, 2 , . . . , n, where i is the 
satellite index, and n denotes the number of receiver channels. 
A set of equations identical to the corresponding equations for the 
propagation of the heading error variance due to scale factor error for 
the enhanced heading filter is required. For example, pseudocode (7), 
which is given for the heading sums, is equally applicable to the 
preferred heading filter. 
The following pseudocode describes propagations for the odometer scale 
factor and slope states: 
If .DELTA.d.gtoreq.d.sub.corr (the slope correlation distance) 
##EQU11## 
If .DELTA.t.gtoreq..tau..sub.corr (the scale factor correlation time), 
##EQU12## 
Propagation of the clock frequency variance is given by 
.sigma..sub..delta.f.sup.2 +=q.sub..delta.f .DELTA.t. 
The propagation of the "consider" state correlations is represented by the 
following pseudocode. 
##EQU13## 
where n is the number of GPS satellites currently being tracked. 
Measurement processing, as described for a single PRR measurement, is as 
follows, 
EQU let PRR measurements be denoted PRR.sub.i.sup.m, i=0, 1 , . . , n. 
The differential correction latency, .DELTA.t.sub.c, should be stored in a 
memory within microcomputer 24 for later use in computing a correlated 
error variance. 
It is assumed that the PRRs have been corrected for satellite vehicle (SV) 
clock frequency error, including relativistic effects, and for SV velocity 
error. If in non-differential mode, different values will be assigned to 
the correlated error variances. When in differential mode and differential 
corrections are not available or have timed-out, it may or may not be 
desirable to continue to use the PRR measurements. If the use of PRR 
measurements is continued, the correlations must be reset. 
Computation of the measurement gradient vector (h), for each PRR 
measurement, is given by, 
EQU h.sup.T ={v[cosH u.sub.e -sinH u.sub.n .vertline.sinH u.sub.e +cosH u.sub.n 
+u.sub.u .delta.S.vertline.u.sub.u ]1} (78) 
where u.sub.e, u.sub.n and u.sub.u are the east, north and up components, 
respectively, of the line-of-sight (LOS) vectors to the SV of interest. 
Residual formation is as follows, 
EQU r.sub.PRR =PRR-h.sup.T x, tm (79) 
where, 
PRR=u.sub.i (v.sub.si -v) (v.sub.s denotes SV velocity, v denotes vehicle 
12 velocity) 
x.sup.T =[.delta.H .delta.S .delta.d.sub.SF .delta.f]. 
Measurement noise variance computation is as follows, 
##EQU14## 
where, .DELTA.t.sub.c =differential correction latency 
.sigma..sub.asa =one sigma SA acceleration=0.005-0.01 m/sec.sup.2. The 
correlated error is dominated by SA. The other correlated errors, e.g., 
ionospheric rate of change, are neglected. These errors can be used to set 
a floor for the propagated correlations. 
Measurement gain calculation is not strictly a Kalman gain, but a modified 
version based on the correlation vector, .sigma..sub.c, as follows, 
EQU .sigma..sub.c.sup.T =[.sigma..sub.c.delta.H .vertline..sigma..sub.c.delta.S 
.vertline..sigma..sub.c.delta.d .vertline..sigma..sub.c.delta.f ]. (83) 
The expression for the Kalman gain (four-dimensional) is, 
EQU k=(Ph-.sigma..sub.c)/(.sigma..sub.PRR.sup.2 +h.sup.t ph-2 .sigma..sub.c 
.multidot.h) =(Ph-.sigma..sub.c)/PRR.sub.resvar. (84) 
A conventional Kalman filter gain calculation results if .sigma..sub.c =0. 
Iterations of the Kalman gain calculation, followed by state, covariance 
and correlation vector update for each satellite are required. 
State update is as follows, 
EQU x+=k r.sub.PRR. (85) 
Measurement deweight/reject pseudocode is, 
##EQU15## 
The covariance update equation is, 
EQU P.sup.+ =(I-kh.sup.T)P.sup.- (I-kh.sup.T).sup.T +kk.sup.T 
.sigma..sub.PRR.sup.2 +(I-kh.sup.T) .sigma..sub.c k.sup.T 
+k.sigma..sub.c.sup.T (I-kh.sup.T).sup.T (87) 
The correlation update equation is, 
EQU .sigma..sub.c.sup.+ =(I-kh.sup.T) .sigma..sub.c.sup.- +k 
.sigma..sup.2.sub.PRR (88) 
The preferred position filter 80 (FIG. 3) operates in a manner analogous to 
the preferred heading and speed filter (69): individual satellite PR 
measurements are processed to estimate and remove the error growth in the 
position propagation, which is based upon the outputs of the heading and 
speed filter. Processing of individual measurements using the four state 
preferred position filter enables "partial updating" of the solution when 
fewer than three satellites are in view. The measurement processing of the 
preferred position filter is nearly independent of the preferred heading 
and speed filter, except that its measurement weighting is coupled to the 
expected accuracies of the heading and speed filter, as will be explained 
in the subsequent discussions. In this way, the two filters realize a set 
of coupled, Kalman-like filters; this formulation has significantly 
reduced computational requirements relative to the use of a fully 
integrated eight state design, by roughly a factor of four. The error 
states of the preferred position filter are defined in Table II. 
TABLE II 
______________________________________ 
Preferred Position Filter 
State Definition 
State Description Use 
______________________________________ 
.delta.p.sub.e 
estimated error 
to be subtracted from 
in the east DR solution 
component of DR 
position in 
meters 
.delta.p.sub.n 
estimated error 
to be subtracted from 
in the north DR solution 
component of DR 
position in 
meters 
.delta.h estimated error 
to be subtracted from 
in the vertical 
DR solution 
component of DR 
position in 
meters 
.delta..phi. 
estimate of to be subtracted from 
user clock DR solution 
phase error 
______________________________________ 
The following ten elements of the full four-by-four covariance matrix are 
propagated: 
##EQU16## 
Thus, propagation equation are needed for 
.sigma..sub..delta.p.sbsb.e.sup.2, .sigma..sub..delta.p.sbsb.e.sub..delta. 
p.sbsb.n, .sigma..sub..delta.p.sbsb.n.sup.2, 
.sigma..sub..delta.p.sbsb.e.sub..delta.h, 
.sigma..sub..delta.p.sbsb.n.sub..delta.h, .sigma..sub..delta.h.sup.2, 
.sigma..sub..delta.p.sbsb.e.sub..delta..PHI., 
.sigma..sub..delta.p.sbsb.n.sub..delta..PHI., 
.sigma..sub..delta.h.delta..PHI., and .sigma..sub..delta..PHI..sup.2, 
based on the outputs from the preferred heading and speed filter. It is 
assumed that the preferred heading and speed filter has run prior to the 
execution of the preferred position filter. Subscripts k and k-1 refer to 
successive updates, or propagations if no PRR measurements are available 
for the speed and heading filter. For example, k is the time at which 
propagation terminates in the position filter, and k-1 is the previous 
propagation termination time. The propagation equations are preferably run 
at one hertz, independent of measurement updating. 
Propagation of the vertical position error variance is derived from 
equations for the altitude propagation, based on the current and previous 
slope and speed estimates. Such equations take the form of: 
##EQU17## 
The resulting altitude variance propagation equation is given by: 
EQU .sigma..sub..delta.h.sup.2 +=.DELTA.d.sub.sum.sup.s 
(.sigma..sub..delta.S.sbsb.avg.sup.2 +S.sup.2 
.sigma..sub..delta.V.sbsb.SFavg.sup.2 +2S.sub.avg 
.sigma..sub..delta.S.delta.v.sbsb.SF) (91) 
where: 
.DELTA..sub.sum.sup.s is computed similarly to the computations for the 
enhanced position filter, e.g., equation (17). 
Propagation of the clock phase error covariance is based upon the following 
equation for the clock phase estimate: 
EQU .delta..PHI..sub.k+1 +.delta..PHI..sub.k +.delta.f.sub.avg .DELTA.t, (92) 
where: 
EQU .delta.f.sub.avg =1/2(.delta.f.sub.k-1 +.delta.f.sub.k). 
The resulting clock error variance propagation equation is given by: 
EQU .sigma..sub..delta..PHI..sup.2 +=q.sub..delta..PHI. 
.DELTA.t+.sigma..sub..delta.favg.sup.2 .DELTA.t.sub.sum.sup.2, (93) 
where: .DELTA.t.sub.sum is computed and controlled the same as 
.DELTA.t.sub.sum.sup.s, but based upon sums of propagation intervals. 
Propagation of the off-diagonal terms: .sigma..sub..delta.pe.delta.h, 
.sigma..sub..delta.pn.delta.h, .sigma..sub..delta.pe.delta..PHI., 
.sigma..sub..delta.pn.delta..PHI., and .sigma..sub..delta.ph.delta..PHI., 
are given by: 
EQU .sigma..sub..delta..PHI..delta.h +=v(.sigma..sub..delta.S.delta.f 
+S.sigma..sub..delta.v.sbsb.SF.sub..delta.f) .DELTA.t.sub.sum.sup.s, (94) 
EQU .sigma..sub..delta..PHI..delta.pe +=v(cos H.sigma..sub..delta.S.delta.f 
+sin H.sigma..sub..delta.v.sbsb.SF.sub..delta.f) .DELTA.t.sub.sum.sup.s, 
(b 95) 
EQU .sigma..sub..delta..PHI..delta.pn +=v(cos 
H.sigma..sub..delta.v.sbsb.SF.sub..delta.f -sin 
H.sigma..sub..delta.H.delta.f) .DELTA.t.sub.sum.sup.s, (96) 
EQU .sigma..sub..delta.h.delta.pe +=[S(cos 
H.sigma..sub..delta.v.sbsb.SF.sub..delta.H +sin 
H.sigma..sub..delta.v.sbsb.SF.sup.2) +cos H.sigma..sub..delta.S.delta.H 
+sin H.sigma..sub..delta.v.sbsb.SF.sub..delta.S ].DELTA.d.sub.sum.sup.s, 
(97) 
EQU .sigma..sub..delta.h.delta.pn +=[S(cos H.sigma..sub..delta.v.sbsb.SF.sup.s 
-sin H.sigma..sub..delta.v.sub.SF.sub..delta.H) +cos 
H.sigma..sub..delta..sbsb.SF.sub..delta.S -sin 
H.sigma..sub..delta.v.sbsb.SF.sub..delta.S ].DELTA.d.sub.sum.sup.s, (98) 
A correlation term for each satellite, representing the effects of SA or SA 
leakage (differential operation) and other residual correlated errors such 
as uncompensated ionospheric delay, must be propagated for each SV: 
EQU .sigma..sub.c.delta.pe.sbsb.1 =.PHI..sub.c, (99) 
EQU .sigma..sub.c.delta.pn.sbsb.i =.PHI..sub.c, (100) 
EQU .sigma..sub.c.delta.h.sbsb.i =.PHI..sub.c, (101) 
EQU .sigma..sub.c.delta..PHI..sbsb.i =.PHI..sub.c, (102) 
where i is the SV index. 
The pseudo range measurements are code/carrier filtered, corrected for SV 
clock and relativistic effects, ionospheric and tropospheric delays, or 
differentially corrected. Measurement residual formation is as follows: 
EQU PR.sub.res.sbsb.i =PR.sub.i.sup.m -R.sub.i 
-.delta..PHI.-u.multidot..delta.p, (103) 
where: 
PR.sup.m is the measured carrier-filtered pseudo range, 
R is the estimated range, based on the propagated position, and 
.delta.p.sup.T =[.delta.p.sub.e, .delta.p.sub.n, .delta.p.sub.h ] 
represents the computed SV positions. 
The measurement gradient vector equation is as follows: 
EQU h.sub.i.sup.T =[u.sub.ei u.sub.ni u.sub.ui 1], (104) 
where: 
i is the SV index, and 
u.sub.e, u.sub.n, u.sub.u are the components of unit line of sight vector 
to SV. 
The Kalman gain calculation is identical in form to that for pseudo range 
rate processing, with the correlation vector .sigma..sub.ci given by, 
EQU .sigma..sub.ci.sup.T =[.sigma..sub.c.delta.pe .sigma..sub.c.delta.pn 
.sigma..sub.c.delta.h .sigma..sub.c.delta..PHI. ]. (105) 
The measurement noise variance is given by, 
EQU .sigma..sub.PR.sup.2 =.sigma..sub.NPR.sup.2 +.sigma..sub.CPR.sup.2, (106) 
where: 
.sigma..sub.NPR.sup.2 represents the noise contribution, 
.sigma..sub.CPR.sup.2 represents the correlated error contribution due 
largely to SA. 
Each component is a function of several receiver-generated parameters: 
EQU .sigma..sub.NPR.sup.2 =f(C/N.sub.o, n), (107) 
where: 
C/N.sub.o =measured signal to noise ratio, 
n=index for code/carrier filter gain, 
##EQU18## 
The covariance update equations and correlation update equations are 
identical in form to those described herein for the heading and speed 
filter, equations (3) and (5). 
The gyro bias filter and associated monitor are preferably included the 
GPS/DR navigation system. The gyro itself generally exhibits drift 
characteristics that can render it unusable without frequent calibration. 
The present invention performs this calibration function each time that a 
vehicle 12 is determined to be stationary, such as indicated by the 
odometer. The calibration can also be done when the vehicle 12 is 
determined to be moving in a straight line, as determined from a very low 
rate signal. This information is input to one of two separate filters, one 
using the stationary information, the second (with a longer smoothing 
time) using the bias information derived while moving. Both filters can be 
realized as fixed-gain, low-pass filters, which are empirically tuned. 
Alternatively, single or two-state Kalman filters may be used in these 
designs. The gyro bias rate, in addition to the gyro bias, is estimated. 
Inclusion of gyro bias rate information can improve the bias estimation 
performance over longer periods of continuous motion, since it can predict 
the change in the determined bias over time. 
Fixed gain bias filtering is as follows: 
EQU b.sub.k+1 =b.sub.k +.alpha.(b.sup.m -b.sub.k), (108) 
where: 
b=bias estimate, 
.alpha.=filter coefficient, and 
b.sup.m =measured bias value. 
The value for .alpha. is a function of the source of the bias measurement. 
A longer time constant which corresponds to a smaller value for .alpha. is 
used if b.sup.m is derived while moving. 
The Kalman bias filter is represented by: 
##EQU19## 
k.sub.b =Kalman gain for the bias estimate, k.sub.b =Kalman gain for the 
bias rate estimate, 
.DELTA.t=time interval between bias measurements. 
The Kalman gains are derived from conventional Kalman equations, based on 
the assignment of a measurement noise variance for the bias measurement, 
which varies according to its source, whether moving or stationary. The 
Kalman gains are further based on an appropriate random walk model for the 
bias-rate state. 
Due to the potential instability of the gyro bias, an a gyro bias monitor 
is preferably included in the gyro bias filter. The gyro bias monitor 
examines the outputs of the bias filter, and injects additional 
uncertainty into the speed and heading filter propagation, which results 
in an increase in the weighting of GPS information, based upon the 
stability of the outputs of the bias filter. The equations of the bias 
filter and its monitor are included below: 
The following pseudocode represents a gyro bias monitor procedure: 
##EQU20## 
else 
EQU q.sub.bias =q.sub.bias -c(q.sub.bias -q.sub.bias nominal) 
where: 
.DELTA.b=b.sub.k+1 -b.sub.k, 
.DELTA.t=time, 
q.sub.bias =process noise parameter used by the speed and heading filter, 
and 
c=coefficient which determines the "settling time" associated with the 
return of q.sub.bias to its nominal value. 
The bias monitor is relatively conservative, e.g., any observed departures 
of the bias from its nominal stability results in an immediate increase in 
the q.sub.bias parameter. The q.sub.bias parameter is only allowed to 
return to its (lower) nominal value after several consistent updates, as 
controlled by "c". 
Depending upon the stability of the gyro scale factors, which are generally 
expected to be strongly temperature dependent, periodic re-calibration 
from their initial values determined at installation may be necessary. The 
calibration generally requires specific maneuvers be performed, which can 
be difficult and time consuming. It is therefore advantageous to adjust 
the scale factor estimates periodically using GPS. Such adaptation is 
risky, since an erroneous scale factor may be difficult to recover from. 
An alternative approach makes use of a GPS-derived estimate of the scale 
factor following each significant heading variation. This avoids 
corruption of the gyro scale factors. The GPS information is weighted 
based upon its expected accuracy, and combined with the scale factor 
determined at installation. In this way, the scale factor will only be 
adjusted if significant evidence indicates that it has changed, e.g., the 
scale factor estimate is desensitized to errant GPS measurements. 
The following pseudocode represents a gyro scale factor update filter 
procedure: 
##EQU21## 
where: 
SF.sup.m =measured positive and negative gyro scale factors, 
.DELTA.H.sub.gyro =gyro measured heading change, 
.DELTA.H.sub.thresh =heading change threshold for scale factor measurement, 
and 
.DELTA.H.sub.GPS =heading change measured by GPS. 
The measured SF.sup.m values are then applied to separate positive and 
negative filters for scale factor, where each is in the following form: 
EQU SF.sub.k+1 =(1-.alpha.)SF.sub.k +.alpha.SF.sup.m.sub.k (115) 
where: 
.alpha.=function of accuracy of GPS measurement, time since last gyro scale 
factor calibration, 
SF.sub.O =value determined at installation or recalibration. 
An alternative to the use of Kalman filtering for the preferred speed and 
heading filter, and the preferred position filter is based on a weighted 
least squares (WLS) algorithm, although the Kalman filter approach is 
presently preferred. One such alternative approach is based upon dual 
four-state WLS filters and closely follows the development of the dual 
four-state Kalman filters described herein for the preferred speed and 
heading and position filters. Therefore, it will not be described to the 
same level of detail. 
The propagation equations for the WLS approach are virtually identical, 
except that propagations are not required for the correlations with GPS 
measurement error (since the WLS filter does not model these effects). The 
measurement update equations are more straightforward than their Kalman 
counterparts, and are summarized by the following equations: 
EQU .DELTA..sub.x =(H.sup.T R.sup.-1 H).sup.-1 H.sup.T R.sup.-1 r 
EQU R=measured error variance (m.times.m) 
EQU r=measured residual vector (m.times.1) 
EQU H=measurement gradient vector (m.times.4) 
EQU .DELTA..sub.x =state correction vector (4.times.1) 
where m=4+n.sub.svs and n.sub.svs =number of GPS satellites. 
The alternative design based upon WLS can be expected, generally, to be 
inferior to the preferred Kalman approach in that it will not be able to 
remove the effects of SA as well. However, its relative simplicity makes 
it an attractive alternative, especially in applications with limited 
processing power. The measurement equations of WLS do permit navigation 
with fewer that three satellites, and identification of faults in both GPS 
and DR. Based upon limited benchmarks for a Motorola type 68332 
microprocessor, the measurement processing of the WLS approach can be up 
to an order of magnitude more efficient than the Kalman approach, and so 
it is worthy of serious consideration as a backup approach if computer 
resources limited. 
Failure detection, as implemented within the WLS filters, is based upon the 
over-determined solution residuals, e.g., the fault vector or parity 
vector. The magnitude of the fault vector can be tested against a 
threshold, and used to detect "failures", or inconsistencies, in the 
redundant measurements. While the direction of the fault vector can be 
used to isolate the faulty measurement(s). If a faulty GPS measurement is 
isolated, it will be removed from the solution. If the position changes or 
the gyro-derived heading are isolated as faulty, a reinitialization to the 
next available GPS solution should occur. The following equations 
summarize the calculation of the fault vector from the over-determined set 
of measurements: 
EQU f=S R.sup.-1/2 r 
where: 
S=fault vector covariance matrix (m.times.m), 
S=I-R.sup.-1/2 H (H.sup.T R.sup.-1/2 H/.sup.-1 H.sup.T R.sup.-1/2, 
f.sup.T f used for fault detection, and 
fi.sup.2 /s.sub.ii used for fault isolation. 
Certain error mechanisms within a DR system can lead to gross navigational 
errors, since they are not reflected in the predicted uncertainty levels 
of Kalman filters. Included in this class of error mechanisms are the 
"ferry problem" and problems induced, for example, by excessive tire 
skidding on the vehicle 12 or an initial excessive heading error. 
Operation of the GPS/DR system onboard a vehicle 12 ferry, or other moving 
platform, will cause excessive errors to develop, since the odometer will 
indicate that the system is stationary. Excessive skidding will similarly 
induce significant error, since the odometer will indicate that the 
vehicle 12 is not moving, when, in fact, it is moving. Modification of the 
basic Kalman equations can be made to improve the tolerance of nonlinear 
effects, as induced by large heading errors, but will not guarantee 
convergence for very large errors. The approach to be taken, as part of 
the present invention, is to reinitialize the GPS/DR solution to the GPS 
only solution when a sufficient number of measurement rejections have 
occurred, indicating that the GPS solution cannot be "spoofed," or be 
based upon reflected signals. Both the position, speed and heading are 
then reinitialized to GPS values, and the GPS/DR filter is restarted. 
Although the present invention has been described in terms of the presently 
preferred embodiments, it is to be understood that the disclosure is not 
to be interpreted as limiting. Various alterations and modifications will 
no doubt become apparent to those skilled in the art after having read the 
above disclosure. Accordingly, it is intended that the appended claims be 
interpreted as covering all alterations and modifications as fall within 
the true spirit and scope of the invention.