diﬀerential wheel speed sensor integration with...

Differential Wheel Speed Sensor Integration with

GPS/INS for Land Vehicle Navigation

Andrew C. Hazlett,∗ John L. Crassidis,† Daniel P. Fuglewicz‡

University at Buffalo, The State University of New York, Buffalo, NY 14260

Patrick Miller§

Calspan Corporation, Buffalo, NY 14225

This paper develops an approach for the integration of GPS, inertial measurements fromaccelerometers and gyros, and differential wheel speed sensors for land vehicle navigation.Incorporating differential wheel speed sensor information into land vehicle navigation pro-vides a solution for eliminating large errors caused by vehicle motions while also reducingerrors from sideslip. Extended Kalman and Unscented filtering algorithms are designedwith a six degree of freedom model. In order to incorporate differential wheel speed infor-mation properly, the effective wheel radius must also be estimated as part of the overallestimation approach. Simulation results show the performance of the filters for cases ofGPS/INS with and without the wheel speed sensor.

I. Introduction

The Global Positioning System (GPS) combined with an Inertial Navigation System (INS) has beenextensively executed for position determination in vehicle navigation.1 GPS alone, with the use of a singlereceiver, can be used to find absolute position up to within a radius of 10 meters.2 Alternate variations,such as Differential GPS (DGPS), which uses a fixed base station, and the Wide Area Augmentation System(WAAS) may allow the position to be found within a meter.3 Even with this high level of precision, aland vehicle cannot use a GPS receiver alone to continuously find position. To compute position using aGPS receiver a signal from at least four satellites must be available for long enough to receive the encodedinformation.4 This transmitted signal is at a frequency of 1.575 GHz, which is too high to pass throughor bend around obstacles such as dense foliage, rugged terrain, tunnels, under bridges or in an urbanenvironment with tall buildings. Due to these obstacles, additional navigation aids are required for mostland vehicle navigation applications. Exceptions to this exist for certain land vehicle applications such asautonomous farm vehicles5 because satellite signals are not impeded in open fields. As of this writing a GPSreceiver costs between $50 and $500 depending on the measurement update rate, which is typically between1 to 10 Hz.3

Navigation with sensors that do not measure absolute position of a vehicle is collectively referred to asdead-reckoning.4 Because dead-reckoning sensors do not measure an absolute position they have errors fromsensor drift that grow without bound if not used in conjunction with an absolute position measurement. Anexample of a dead-reckoning sensor is an INS consisting of gyros and accelerometers. In this paper anotherdead-reckoning sensor involving differential wheel speed measurements is considered for land navigation.The use of dead-reckoning may be described as a flywheel to keep accurate position measurements for shortterm GPS outages and to smooth the low update rate of GPS. Over long periods of time GPS is required to

∗Student, Aerospace Engineering, Email: [email protected], Student Member AIAA.†Professor, Department of Mechanical and Aerospace Engineering, Email: [email protected], Associate Fellow AIAA.‡Senior Engineering Manager, Experimental Campus for Large Infrastructure Protection, Sustainability, and Enhancement

(ECLIPSE), Multidisciplinary Center for Earthquake Engineering (MCEER), Department of Civil, Structural and Environ-mental Engineering, Email: [email protected].

§Senior Engineer, Systems Engineering Group, 4455 Genesee Street, Email: [email protected].

1 of 20

American Institute of Aeronautics and Astronautics

calibrate the dead-reckoning sensor drift. The fusion of GPS and dead-reckoning sensors counter limitationsthat exist in each, creating an advantage to using them in conjunction.

With the advent of less expensive INS sensors, especially Micro-Electrical Mechanical Systems (MEMS),it is now a reality to use these sensors for land vehicle navigation applications. These less expensive sensorsdo not perform as well as high-accuracy sensors in terms of drift and white-noise measurement errors, butare compensated for when combined with GPS.6 The INS sensors considered in the present research includeaccelerometers and rate gyroscopes. Currently inexpensive MEMS accelerometers and gyros cost less than$30, with update rates on the order of 100 Hz.3

The rear wheel speed sensors incorporated into the present research provides measurements of the vehicle’syaw rate and the velocity in the vehicle’s heading direction. Wheel speed sensors currently exist as part of anAnti-lock Brake System (ABS), which is a standard feature on many present day land vehicles.7 The wheelspeed sensor integration into navigation systems to find position has been studied by Kubo and Gao.8, 9 Inthis previous work two conditions must be met in order for the integration of the wheel speed sensor toproduce valid results; the vehicle must operate on a flat road and no side slip may occur.9 These conditionsdo not always hold, such as the large side slip that exists when a vehicle travels on a bumpy surface oroff-road. Note that side-slip is also a function of yaw rate, speed, and vehicle weight distribution. Sideslip occurs instantaneously, and is not easily modeled. Much research has been conducted into creatingapproximations for modeling side slip.5, 10 Another method used to avoid the errors caused by side slip isto detect it and use an adaptive estimation algorithm that bypasses the wheel speed measurement when anunacceptable level occurs. An additional error source is created from pressure variations in the tire. Thismay be accounted for by including each wheel radius as a state variable. Without this, large errors occurwhen the rear wheel radii vary, such as the case of using a spare tire where the wheel radius may changeover 5 cm.11

In navigation the most common method of integrating GPS and INS has been with an Extended KalmanFilter (EKF).12 A well-known drawback of the EKF is that if the errors are not within the linear region,the filter may diverge. This is especially relevant with GPS/INS integration due to unknown attitude andinertial sensor calibration parameters a priori. One aspect of the EKF algorithm design is the method of howa measurement is processed, which may be categorized in two categories; loosely or tightly coupled.6 Tightlycoupled refers to when the measurement is directly used in the filter as it is provided by the sensor. Looselycoupled uses an estimated value of a current state in the filter that is found from the sensor output. Theloose-tight coupling distinction is independent for each measurement used in the filter. As for a GPS sensora tightly coupled configuration allows for position information to be provided with a minimum of only threesatellite signals but also requires knowledge of variables used in the tracking loops that may not be readilyavailable.6 In the present research the GPS measurement is integrated into the EKF using a loosely coupledapproach, in which a minimum of four GPS satellite signals must be available. This requires independentposition estimates from GPS to be used in the EKF as measurements. Alternately, the differential wheelspeed sensor measurements are used in a tightly coupled manner. This allows for no pre-processing of themeasurements from the wheel speed sensors to be preformed and has been proven an effective approach byCarlson.11

In addition to the EKF an Unscented Filter (UF) is considered in the present research. The UF essentiallyprovides derivative-free higher-order approximations by approximating a Gaussian distribution rather thanapproximating an arbitrary nonlinear function as in the EKF.13 This provides several advantages over theEKF; a lower expected error, use on non-differentiable functions, no Jacobian matrix calculation and validityfor higher order expansions.6

The organization of this paper is as follows. The various coordinate frames used in INS includingtransformations between frames are discussed in Section II. Section III provides the background for theattitude kinematics used in the estimation algorithm. The linearized equations are shown for the INSequations along with the measurement models for the inertial sensors in Section IV. The differential wheelspeed sensor measurement model is defined in Section V. Section VI summarizes the states, measurementsand linearized equations used to propagate the states in the filters. The EKF and UF algorithms are thendefined in Sections VII and VIII. This leads to the simulation results and discussion of initial conditionerrors in Section IX.

2 of 20


II. Reference Frames

A. Earth-Centered-Inertial (ECI) Frame

The ECI frame is denoted by i1, i2, i3 and the vectors described in this frame have superscript I (e.g., rI .)For the purpose of Earth based navigation this frame is considered inertial. This frame does not rotate withrespect to the stars and has an origin at the center of Earth. The i3 axis is aligned in the direction of Earth’sNorth pole, the i1 axis is aligned in the vernal equinox direction and the i2 axis completes the right handedsystem.

B. Earth-Centered-Earth-Fixed (ECEF) Frame

The ECEF frame is denoted by e1, e2, e3 and vectors described in this frame have superscript E (e.g., rE .)The ECEF frame has an origin at the center of Earth and is fixed to the Earth’s motion, rotating with theEarth. For this frame e3 = i3, e1 is in the direction of Earth’s prime meridian and e2 completes the righthanded system. In this frame the e1 and e2 axes are rotated by Θ from the ECI frame.

C. Transformation From ECI to ECEF

The transformation between the ECI and ECEF frame involves only a single-axis rotation about i3. This isdefined by

x

y

z

E

=

cosΘ sinΘ 0

− sinΘ cosΘ 0

0 0 1

x

y

z

I

(1)

The angle Θ defines this transformation and can be found using the sidereal day. The solar The conversionfrom Universal Time Coordinated (UTC) to Greenwhich Mean Sidereal Time (GMST) is given by Meeus.14

D. Local North-East-Down (NED) Frame

The NED frame is denoted by n, e, d and vectors described in this frame have the superscript N (e.g.,rN .) This frame is used for local navigation due to the frame having an origin at the geodetic point of

interest and the frame forming a plane normal to the surface of the Earth. The d axis points to the centerof the Earth, the n axis points in the true North direction on Earth’s surface and e axis points true Easton Earth’s surface. This coordinate system also has the advantage of allowing the acceleration due to theEarth’s gravity to be entirely in the d direction.

E. Transformation From ECEF to NED

This transformation can be described as the combination of first a rotation from the ECEF frame to anEast-North-Up (ENU) frame using latitude (λ) and longitude (Φ), and then a simple change in axes from

the ENU to NED frames. This change in axes is described by nN = eENU , eN = nENU and dN = −uENU ,allowing for a right hand system with the third component in the down direction. This rotation from ECEFto NED can be described with

ANE =

− sinλ cosΦ − sinλ sinΦ cosλ

− sinΦ cosΦ 0

− cosλ cosΦ − cosλ sinΦ − sinλ

(2)

F. Body Frame

The body frame is denoted by b1, b2, b3 and is described by the superscript B (e.g., rB .) This frame isfixed to and rotating with the body of interest.

3 of 20


G. Transformation From NED to Body

This last transformation is found from a 3-2-1 Euler angle rotation from the body to the NED frame. Thisrotation is defined by φ, θ and ψ, which are roll, pitch, and yaw, respectively. The following equation definesthis relationship, showing the attitude matrix from the body-frame to the NED frame:

ANB =

cos θ cosψ − cosφ sinψ + sinφ sin θ cosψ sinφ sinψ + cosφ sin θ cosψ

cos θ sinψ cosψ cosφ+ sinφ sin θ sinψ − sinφ cosψ + cosφ sin θ sinψ

− sin θ sinφ cos θ cosφ cos θ

(3)

H. Geodetic Coordinate System (LLH)

Geodetic coordinates are the navigational coordinate system used by GPS to navigate on the surface of Earth.Geodetic coordinates consist of latitude, longitude, and height (LLH), denoted as λ,Φ, h. Latitude is theangle from the equator to the position of interest on Earth. Longitude is the angle measured from the primemeridian around the Earth to the location. The height represents the difference from the Earth’s ellipsoid tothe location, which is also known as altitude. This ellipsoid is approximated in the World Geodetic System1984 (WGS-84) model.15 Pertinent parameters for this model are shown in Table 1. These parametersare used in the transformation of coordinate frames from ECEF to LLH and back. This transformation isbetween different types of coordinate systems and thus has increased complexity from the attitude matricesthat give the other coordinate transformations in this section. The transformation from LLH to ECEF ismuch simpler than the transformation from ECEF to LLH.

Table 1. WGS-84 Model Parameters

Parameter Value Description

a 6378137.0 m Earth ellipsoid semi-major axis

b 63563124.2 m Earth ellipsoid semi-minor axis

e 0.0818 Earth eccentricity

ωe 7.292115× 10−5 rad/sec Earth rotation rate

I. Transformation From LLH to ECEF

The conversion from LLH to ECEF coordinates begins with the calculation of N , the length vector from thecenter of Earth’s ellipsoid to the surface:12

N =a

√

1− e2 sin2 λ(4)

The ECEF coordinates can now be directly calculated using the following equations and the parametersfrom Table 1:

x = (N + h) cosλ cosΦ (5a)

y = (N + h) cosλ sinΦ (5b)

z = [N(1− e2) + h] sinλ (5c)

J. Transformation From ECEF to LLH

The conversion back to LLH is more complicated and is shown in the closed form solution from Zhu16 usingthe parameters of Table 1:

w =√

x2 + y2, l =e2

2, m = (

w

a)2 (6a)

n = [(1− e2)z/b]2, i = −(2l2 +m+ n)/2, k = l2(l2 −m− n) (6b)

q = (m+ n− 4l2)3/216 +mnl2, D =√

(2q −mnl2)mnl2 (6c)

4 of 20


β = i/3− 3√

q +D − 3√

q −D (6d)

t =

√

√

β2 − k − (β + i)/2− sign(m− n)√

(β − i)/2 (6e)

w1 = w/(t+ l), z1 = (1 − e2)z/(t− l) (6f)

φ = atan(z1/((1− e2)w1)) (6g)

λ = 2atan[(w − x)/y] (6h)

h = sign(t− 1 + l)√

(w − w1)2 + (z − z1)2 (6i)

For completeness, for the special case of being located at one of the Earth’s poles, i.e. when w = 0, theheight and latitude must be calculated as follows:

h = sign(z)z − b, φ = sign(z)π/2 (7)

III. Attitude Kinematics

This section summarizes the attitude kinematics that are used in the filter design. The attitude ma-trix consists of (3 × 3) dependent components that map the rotation from one frame to another. Theattitude matrix can be parameterized using a smaller set of elements to represent the matrix. Methodsof parameterizations include Euler axis/angle rotations, Euler angles, quaternions, and the Gibbs vector.The parameterization chosen is problem dependent based which method is most advantageous. Each of theparameterizations are equivalent and may be converted between one another.17

In this paper the quaternion is used extensively in the filter design due to the advantage of the existenceof no singularities, no transcendental functions, and because successive rotations may be taken by quaternionmultiplication. The attitude parameterization of the quaternion uses a four-dimensional vector which mustsatisfy the constraint qTq = 1. The quaternion is defined as q = [T q4]

T , where = [q1 q2 q3]T . The

attitude matrix is related to the quaternion by the following equation:

A(q) = ΞT (q)Ψ(q) (8)

in which

Ξ(q) ≡[

q4I4×4 + [×]−T

]

, Ψ(q) ≡[

q4I4×4 − [×]−T

]

(9)

The cross product matrix, [×], is an operator defined as

[×] =

0 −q3 q2

q3 0 −q1−q2 q1 0

(10)

The attitude kinematics equation for the body to NED frame is given by

ANB = −[ωBB/N×]ABN (11)

The attitude kinematics equation for the NED to body frame is given by

ABN = ANB [ωBB/N×] (12)

which is used to find the INS equations. From this, the quaternion kinematic relationship is given as

q =1

2Ξ(q)ωBB/N =

1

2Ω(ωBB/N )q (13)

with

Ω(ωBB/N ) ≡[

−[ωBB/N×] ωBB/N

−(ωBB/N )T 0

]

(14)

Additionally the inverse quaternion is defined as

q−1 ≡[

−q4

]

(15)

5 of 20


IV. INS Equations

The inertial navigation equations used to propagate the states in the filters are defined in the NEDcoordinate system and include the quaternion parameterization:

q =1

2Ξ(q)ωBB/N (16a)

λ =υN

Rλ + h(16b)

Φ =υE

(RΦ + h) cosλ(16c)

h = −υD (16d)

υN = −[ υE(RΦ + h) cosλ

+ 2ωe]

υE sinλ+υNυDRλ + h

+ aN (16e)

υE = −[ υE(RΦ + h) cosλ

+ 2ωe]

υN sinλ+υEυDRΦ + h

+ 2ωeυD cosλ+ aE (16f)

υD = − υ2ERΦ + h

− υ2NRλ + h

− 2ωeυE cosλ+ g + aD (16g)

The following equations define the Earth’s surface based on the latitude and longitude:

Rλ =a(1− e2)

(1− e2 sin2 λ)3/2(17a)

RΦ =a

(1− e2 sin2 λ)1/2(17b)

The local gravity can be calculated as

g = 9.780327(1+ 5.3924× 10−3 sin2 λ− 5.8× 10−6 sin2 2λ)

− (3.0877× 10−6 − 4.4× 10−9 sin2 λ)h+ 7.2× 10−14h2(18)

The inertial measurements used in this paper come from a 3-axis rate gyroscope and a 3-axis accelerome-ter. When modeling both of these sensors a bias drift and white noise must be included to accurately depictthe sensor properties. The measurement model for the rate gyroscope is given by

ωBB/I = ωBB/I + bg + ηgv (19)

bg = ηgu (20)

where bg is the gyroscope bias, and ηgu and ηgv are zero-mean Gaussian white-noise processes with spectraldensities respectively given by σ2

guI3×3 and σ2gvI3×3. The measurement model for the accelerometer in body

coordinates is given as follows:aB = aB + ba + ηav (21)

ba = ηau (22)

where ba is the accelerometer bias, and ηau and ηav are zero-mean Gaussian white-noise processes withspectral densities respectively given by σ2

auI3×3 and σ2avI3×3.

The measurement taken by the gyroscope cannot be directly used in the INS equations but can beconverted from the ECI frame to the NED using

ωBB/I = ωBB/N + ωBN/I (23)

where ωBB/N can be found from

ωBN/I = ABN (q)ωNN/I (24)

6 of 20


and ωNN/I is defined as

ωNN/I = ωNE/I + ωNN/E = ωe

cosλ

0

− sinλ

+

υE

RΦ+h

− υN

Rλ+h

−υE tanλRΦ+h

(25)

The acceleration in the NED frame is related to the acceleration in the body-frame from the relationship

aN ≡

aN

aE

aD

= ANB (q)aB (26)

Note that ANB (q) is the matrix transpose of ABN (q).

V. Wheel Speed Sensor Model

The model used to find the measurements for the speed of each of the rear wheels assumes the velocityin heading direction of the vehicle is entirely in the first component of the fixed in the body frame, so that

vB =

2C1Vb

0

0

(27)

where C1 is a constant that defines the time increment in which the number of wheel rotations are to becounted, and Vb is an interim variable which takes the average value of the left/right wheel speeds and radiisize. These variables are given by

C1 =π

100 (sec)(28a)

Vb =rL|L|+ rR|R|

2(28b)

where rL and rR are the respective left and right wheel radii, and |L| and |R| are the respective left and rightwheel counts. Note that in the present research all wheel speeds measurements are found as the numberof rotations per 100 seconds as defined in Eq. (28a). Due to the wheel speeds being tightly coupled, thewheel count per 100 seconds is the direct sensor measurement used in the filter and the wheel radii must beincluded as states. The velocity is next converted to the NED frame using the attitude matrix calculatedfrom the quaternion:

vN ≡

vN

vE

vD

= ANB (q)vB (29)

The wheel count sensors also give the additional information of the yaw rate based on the difference in wheelrotation counts on the right and left side of the vehicle.

ψ = 2C1rL|L| − rR|R|

b(30)

In this equation b = 1 (m) and is the half length of the vehicle’s rear axle. The yaw rate can be related tothe attitude of the vehicle by converting the rotation to the angular rate between the NED and body-frameby using the attitude kinematics relationship. This relationship is

φ

θ

ψ

= S−1(φ, θ)ωBB/N (31a)

S−1(φ, θ) =

1 sinφ tan θ cosφ tan θ

0 cosφ − sinφ

0 sinφ sec θ cosφ sec θ

(31b)

7 of 20


Using this model the measurements for the wheel count on the left and right side can be determined:

|L| = 1

2C1rLABN(1, 1)vN +ABN (1, 2)vE +ABN (1, 3)vD+

b

4C1rL(sin φ sec θ ω2 + cosφ sec θ ω3) (32a)

|R| = 1

2C1rRABN (1, 1)vN +ABN (1, 2)vE +ABN (1, 3)vD −

b

4C1rR(sinφ sec θ ω2 + cosφ sec θ ω3) (32b)

where ABN (i, j) is the i-jth element of ABN .

VI. Estimation Vector, Measurement Vector and System Model

The estimation vector for this study has 18 states, representing attitude, position, velocity, inertialsensor bias and wheel radii. When the alternate attitude parameterization of Euler angles are used, the fourquaternion states are represented in three angles such as in the results section of this paper. The followingvector displays these states:

x ≡

q

p

vN

bg

ba

rL

rR

(33)

The attitude matrix from the body to NED frame is represented by the four states of a quaternion param-eterization, q. Position has three states and is given in geodetic coordinates, p ≡ [λ Φ h]T . Velocity alsoconsists of three states and is in the NED frame, vN . Both the rate gyroscope and accelerometer each havethree states representing the biases ,bg and ba. The left and right wheel radii are the two remaining states,rL and rR. Measurements for 17-state filter are given by

y ≡

pGPS

ωBB/I

aB

|L||R|

(34)

The system model is used by the filters for estimating the states from the measurements, predicting thecovariance of the filter and to demonstrate observability. For the combined GPS/INS/wheel speed sensorthe following equations represent the model used to propagate the states.

˙q =1

2Ξ(q)ωBB/N (35a)

˙λ =

υN

Rλ + h(35b)

˙Φ =

υE

(RΦ + h) cos λ(35c)

˙h = −υD (35d)

˙υN = −[ υE

(RΦ + h) cos λ+ 2ωe

]

υE sin λ+υN υD

Rλ + h+ aN (35e)

˙υE = −[ υE

(RΦ + h) cos λ+ 2ωe

]

υN sin λ+υE υD

RΦ + h+ 2ωeυD cos λ+ aE (35f)

˙υD = − υE2

RΦ + h− υN

2

Rλ + h− 2ωeυE cos λ+ g + aD (35g)

8 of 20


˙bg = 0 (35h)

˙ba = 0 (35i)

˙rL = 0 (35j)

˙rR = 0 (35k)

A 15-state filter is also used that does not incorporate wheel speed measurements. Wheel radii are notestimated in this filter. The state and measurement vectors for this filter is given by

x ≡

q

p

vN

bg

ba

, y ≡

pGPS

ωBB/I

aB

(36)

This filter is used to compare the results with and without wheel speed measurements.

Table 2. Extended Kalman Filter for GPS/INS/Wheel Speed Sensor Estimation

Initialize x(t0) = x0

P (t0) = P0

Gain Kk = P−k H

Tk [HkP

−k H

Tk + Rk]

−1

Update P+k = [I −KkHk]P

−k

∆x+k = Kk[pk − p−

k ]

q+k = q−

k + 12Ξ(q

−k )δα

+k

p+k = p−

k +∆p+k

vN+k = vN−

k +∆vN+k

b+gk = b−

gk +∆b+gk

b+ak = b−

ak +∆b+ak

r+L = r−L +∆r+Lr+R = r−R +∆r+R

Propagation ωBB/N = ωBB/I − bg −ABN (q)ωNN/i˙q = 1

2Ξ(q)ωBB/N

aB = aB − ba˙p = fp(p, v

N )˙vN = fv(p, v

N ) + aN

P = FP + PFT +GQGT

VII. Extended Kalman Filter Design

The 17-state EKF estimation algorithm for the GPS/INS/wheel speed sensor combination is summarizedin Table 2. This design uses the GPS position measurements loosely coupled and the wheel speed sensormeasurements tightly coupled. The state-error, process noise vector and process noise covariance matrix

9 of 20


used in the EKF estimation algorithm are defined as

∆x ≡

δα

∆p

∆vN

∆bg

∆ba

∆rL

∆rR

, w ≡

ηgv

ηgu

ηav

ηau

, Q =

σ2gvI3×3 03×3 03×3 03×3

03×3 σ2guI3×3 03×3 03×3

03×3 03×3 σ2avI3×3 03×3

03×3 03×3 03×3 σ2auI3×3

(37)

where δα is the incremental attitude error.18

The sensitivity matrix for the EKF is defined as the partial derivatives of the states related to themeasurements, given by

Hk =

03×3∂pECEF

∂pLLH03×3 03×3 03×3 03×1 03×1

∂|L|∂α 01×3

∂|L|∂vN 01×3 01×3

∂|L|∂rL

∂|L|∂rR

∂|R|∂α 01×3

∂|R|∂vN 01×3 01×3

∂|R|∂rL

∂|R|∂rR

(38)

The partial derivatives of the position in ECEF coordinates with respect to the position in geodetic coordi-nates are found from a covariance mapping between these two coordinate systems, given by Ref. 6:

∂pECEF

∂pLLH=

∂N∂λ cosλ cosΦ− (N + h) sinλ cosΦ −(N + h) cosλ sinΦ cosλ cosΦ∂N∂λ cosλ sinΦ− (N + h) sinλ sinΦ (N + h) cosλ cosΦ cosλ sinΦ

∂N∂λ (1− e2) sinλ+ [N(1− e2) + h] cosλ 0 sinλ

(39)

where∂N

∂λ=

ae2 sinλ cosλ

(1− e2 sin2 λ)3/2(40)

The partial derivatives with respect to each of the states in this matrix are found for the left wheel from thewheel count measurement equation. A perturbation approach is used to approximate the partial derivativesfor components of the attitude matrix.18 This approach uses Eq. (41) to find the partial derivatives directlyfrom attitude matrix components as opposed to taking partial derivatives of the Euler angle componentsfrom Eq. (3):

∆vB

δα= [ABN (q)vN×] (41)

Using this approach the partial derivatives are

∂|L|∂φ

=b

4C1rL(cosφ sec θ ω2 − sinφ sec θ ω3) (42a)

∂|L|∂θ

=b

4C1rL(sinφ sec θ tan θ ω2 + cosφ sec θ tan θ ω3)−

1

2C1rL(ABN (3, 1)vN +ABN (3, 2)vE +ABN (3, 3)vD)

(42b)∂|L|∂ψ

=1

2C1rL(ABN (2, 1)vN +ABN (2, 2)vE +ABN (2, 3)vD) (42c)

∂|L|∂α

=[

∂|L|∂φ

∂|L|∂θ

∂|L|∂ψ

]

(42d)

∂|L|∂vN

=ANB (1, 1)

2C1rL,

∂|L|∂vE

=ANB (1, 2)

2C1rL,

∂|L|∂vD

=ANB (1, 3)

2C1rL(42e)

∂|L|∂vN

=[

∂|L|∂vN

∂|L|∂vE

∂|L|∂vD

]

(42f)

∂|L|∂rL

=−1

2C1r2L(ANN (1, 1)vN +ANB (1, 2)vE +ANB (1, 3)vD) +

−b2C1r2L

(sinφ sec θω2 + cosφ sec θω3) (42g)

10 of 20


∂|L|∂rR

= 0 (42h)

Similarly the partial derivatives for the right wheel are

∂|R|∂φ

=−b

4C1rR(cosφ sec θω2 − sinφ sec θω3) (43a)

∂|R|∂θ

=−b

4C1rR(sinφ sec θ tan θω2 + cosφ sec θ tan θω3)−

1

2C1rR(ABN (3, 1)vN +ABN (3, 2)vE +ABN (3, 3)vD)

(43b)∂|R|∂ψ

=1

2C1rR(ABN (2, 1)vN +ABN (2, 2)vE +ABN (2, 3)vD) (43c)

∂|R|∂α

=[

∂|R|∂φ

∂|R|∂θ

∂|R|∂ψ

]

(43d)

∂|R|∂vN

=ANB (1, 1)

2C1rR,

∂|R|∂vE

=ANB (1, 2)

2C1rR,

∂|R|∂vD

=ANB (1, 3)

2C1rR(43e)

∂|R|∂vN

=[

∂|R|∂vN

∂|R|∂vE

∂|R|∂vD

]

(43f)

∂|R|∂rR

=−1

2C1r2R(ANN (1, 1)vN +ANB (1, 2)vE +ANB (1, 3)vD) +

b

2C1r2R(sinφ sec θω2 + cosφ sec θω3) (43g)

∂|R|∂rL

= 0 (43h)

The error dynamics for the EKF propagation are given as

∆x = F∆x+Gw (44)

of which the F and G matrices are defined as

F =

F11 F12 F13 −I3×3 03×3 01×3 01×3

03×3 F22 F23 03×3 03×3 01×3 01×3

F31 F32 F33 03×3 F35 01×3 01×3

03×3 03×3 03×3 03×3 03×3 01×3 01×3

03×3 03×3 03×3 03×3 03×3 01×3 01×3

03×1 03×1 03×1 03×1 03×1 0 0

03×1 03×1 03×1 03×1 03×1 0 0

(45)

G =

−I3×3 03×3 03×3 03×3

03×3 03×3 03×3 03×3

03×3 03×3 −ANB (q) 03×3

03×3 I3×3 03×3 03×3

03×3 03×3 03×3 I3×3

01×3 01×3 01×3 01×3

01×3 01×3 01×3 01×3

(46)

The components of the F matrix are

F11 = −[(ωBB/I − bg)×], F12 = −ABN (q)∂ωNN/I

∂p

∣

∣

p,vN , F13 = −ABN (q)∂ωNN/I

∂vN

∣

∣

p(47a)

F22 =∂p

∂p

∣

∣

p,vN , F23 =∂p

∂vN∣

∣

p(47b)

F31 = −ABN(q)[aB×], F32 =∂vN

∂p

∣

∣

p,vN , F33 =∂vN

∂vN∣

∣

p,vN , F35 = −ABN (q) (47c)

11 of 20


In these components the partial derivatives for position are

∂p

∂p=

υN

(Rλ+h)2∂Rλ

∂λ 0 − υN

(Rλ+h)2

− υE secλ(RΦ+h)2

∂RΦ

∂λ + υE secλ tanλRΦ+h 0 − υE secλ

(RΦ+h)2

0 0 0

(48)

∂p

∂vN=

1Rλ+h

0 0

0 secλRΦ+h 0

0 0 −1

(49)

and the partial derivatives for velocity are given by

∂vN

∂p=

Y11 0 Y13

Y21 0 Y23

Y31 0 Y33

,

∂vN

∂vN=

Z11 Z12 Z13

Z21 Z22 Z23

Z31 Z32 0

(50)

where

Y11 = −υ2E sec2 λ

RΦ + h+

υ2E tanλ

(RΦ + h)2∂RΦ

∂λ− 2ωeυE cosλ− υNυD

(Rλ + h)2∂Rλ∂λ

(51a)

Y13 =υ2E tanλ

(RΦ + h)2− υEυD

(Rλ + h)2(51b)

Y21 =υEυN sec2 λ

RΦ + h− υEυN tanλ

(RΦ + h)2∂RΦ

∂λ+ 2ωeυN cosλ− υEυD

(RΦ + h)2∂RΦ

∂λ− 2ωeυD sinλ (51c)

Y23 = −υE[υN tanλ+ υD

(RΦ + h)2)

]

(51d)

Y31 =υ2E

(RΦ + h)2∂RΦ

∂λ+

υ2N(Rλ + h)2

∂Rλ∂λ

+ 2ωeυE sinλ+∂g

∂λ(51e)

Y33 =υ2E

(RΦ + h)2+

υ2N(Rλ + h)2

+∂g

∂λ(51f)

and

Z11 =υD

Rλ + h, Z12 = −2υE tanλ

RΦ + h+ 2ωe sinλ Z12 =

υNRλ + h

(52a)

Z21 =2υE tanλ

RΦ + h+ 2ωe sinλ, Z22 =

υD + υN tanλ

RΦ + h, Z23 =

υERΦ + h

+ 2ωe cosλ (52b)

Z31 = − 2υNRλ + h

, Z32 = − 2υERΦ + h

− 2ωe cosλ (52c)

with the partial derivatives for local gravity given by

∂g

∂λ= 9.780327[1.06048× 10−2 sinλ cosλ− 4.64× 10−5(sinλ cos3 λ− sin3 λ cosλ)] + 8.8× 10−9h sinλ cosλ

(53a)∂g

∂h= −3.0877× 10−6 + 4.4× 10−9 sin2 λ+ 1.44× 10−13h (53b)

In the estimation algorithm the assumed measurements for position from the GPS are modeled by

pk = pk + vk (54)

where vk is a zero-mean Gaussian noise process with covariance Rk. The filter is first initialized with aknown state (the bias initial conditions for the gyro and accelerometer are usually assumed zero) and theerror-covariance matrix. The first three diagonal elements of the error-covariance matrix correspond to theattitude errors. Then, the Kalman gain is computed using the measurement-error covariance Rk and thesensitivity matrix. The state error-covariance follows the standard EKF update. The position, velocity,

12 of 20


bias and wheel radius states also follow the standard EKF additive correction while the attitude error-stateupdate is computed using a multiplicative update.18 The updated quaternion is re-normalized by bruteforce. The propagation equations follow the standard EKF model.

The P equation for the propagation of the covariance can be approximated by using discrete time prop-agation given by

P−k+1 = ΦkP

+k ΦTk +Qk (55)

In this equation Qk is the discrete time covariance matrix and Φk is the transition matrix. These can benumerically solved for by using Moler and Van Loon.19 In this procedure the following (2n× 2n) matrix iffirst found:

A =

[

−F GQGT

0 FT

]

∆t (56)

From A the matrix exponential is computed:

B = eA ≡[

B11 B120 B22

]

=

[

B11 Φ−1k Qk

0 ΦTk

]

(57)

From this equation the state transition matrix can be found from

Φk = BT22 (58)

and process noise covariance found fromQk = ΦkB12 (59)

VIII. Unscented Filter Design

The filter presented is from Wan,20 derived for discrete-time nonlinear equations, in which the systemmodel is given as

xk+1 = f(xk, k) +wk (60a)

y = h(xk, k) + vk (60b)

A continuous-time model can be written using Eq. (60a) with an appropriate numerical integration scheme.It is assumed that wk and vk are zero-mean Gaussian noise processes with covariance given by Qk and Rkrespectively. First the Kalman filter update equation is rewritten as21

x+k = x−

k +Kkυk (61a)

P+k = P−

k +KkPυυk KT

k (61b)

where υk is the innovations process given by

υk ≡ yk − y−k (62a)

= yk − h(x−k , k) (62b)

The covariance of υk is defined as P ννk . The gain Kk is next found:

Kk = P xyk (P υυk )−1 (63)

where P xyk is the cross-correlation covariance between x−k and y−

k .The propagation equations begin with the calculation of the sigma points:

σk ← 2n columns from±√

P+k + Qk (64a)

χk(0) = x+k (64b)

χk+1(i) = σk(i) + x+k (64c)

13 of 20


Note that there are now a total of 2n sets of sigma points σk, from the positive and negative square roots.Each set of sigma points is evaluated through

χk+1(i) = f [χk(i), k] (65)

The sigma points are combined to find the predicted mean for the state estimate by using a weighted sumof χk+1(i), given by

x−k+1 =

2n∑

i=0

Wmeani χk+1(i) (66)

This predicted mean is now used to find the predicted covariance:

P−k+1 =

2n∑

i=0

W covi [χk+1(i)− x−

k+1][χk+1(i)− x−k+1]

T + Qk (67)

The mean observation is given as

y−k+1 =

2n∑

i=0

Wmeani γk+1(i) (68)

whereγk+1(i) = h[χk+1(i), k + 1] (69)

The output covariance is given as

P yyk+1 =

2n∑

i=0

W covi [γk+1(i)− y−

k+1][γk+1(i)− y−k+1]

T (70)

The innovations covariance can simply be found by

P υυk+1 = P yyk+1 +Rk+1 (71)

Lastly the cross-correlation matrix is found from

P xyk+1 =2n∑

i=0

W covi [χk+1(i)− x−

k+1][γk+1(i)− y−k+1]

T (72)

The parameters required in this algorithm are next discussed. The parameter γ is given by the followingequation in which λ is a composite scaling parameter:

γ =√n+ λ (73a)

λ = α2(n+ κ)− n (73b)

In this equation α is a constant which determines the spread of the sigma points and is set to a smallpositive number between, 1 × 10−4 ≤ α ≤ 1. The scalar κ is chosen to be κ = n − 3 which minimizesmean-squared-error to fourth order.

The weights used in the covariance calculations are found from

Wmean0 =

λ

n+ λ(74a)

W cov0 =

λ

n+ λ+ (1 − α2 + β) (74b)

Wmeani =W cov

i =1

2(n+ λ), i = 1, 2, ..., 34 (74c)

Similarly to the EKF the Unscented filter requires the calculation of the discrete time state transition matrixand covariance noise matrix to add the process noise in the filter. This requires that the F and G matrix becalculated. The equation used to calculate the process noise is as follows:

Φ(∆t)QkΦT (∆t) + Qk = Qk (75)

This equation is known as the discrete time Sylvester equation and is solved with a Bartels-Stewert numericalalgorithm.22

14 of 20


A. Unscented Filter Design for GPS/INS/Wheel Speed Sensors

This section discusses deriving the UF for specific GPS/INS/wheel speed sensor application. The estimationalgorithm follows closely the UF filter description above, with a quaternion normalization procedure. Whenthe predicted quaternion mean is derived using the averaged sum of quaternions the resulting quaternionmay not have unit norm. This creates a situation in which the straightforward calculation in the UF isundesirable for the quaternion. A three-component vector representing an attitude error is used instead.

To begin the state vector is defined as

χk(0) = x+k ≡

δs+kp+k

pN+k

bg

ba

rL

rR

, χk(i) ≡

χδsk (i)

χpk(i)

χvN

k (i)

χbgk (i)

χbak (i)

χrLk (i)

χrRk (i)

(76)

where δs+k is a generalized Rodrigues error-vector,23 used in propagating and update of the nominal quater-nion. Because the three-dimensional attitude is unconstrained, the resulting overall covariance matrix is a17× 17 matrix. The three components of the vector χk(i) are defined in Eq. (64) as χδsk (i). To describe χδska new quaternion is generated by multiplying an error quaternion by the current estimate:

q+k (0) = q+

k (77a)

q+k (i) = δq+

k (i)⊕ q+k (77b)

where δq+k (i) ≡

[

δ+Tk (i) δq+T4k

(i)]

represented by23

δq+T4k(i) =

−a‖χδsk (i)‖2 + f√

f2 + (1− a2)‖χδsk (i)‖2

f2 + ‖χδsk (i)‖2 , i = 1, 2..., 34 (78a)

δ+k (i) = f−1[a+ δq+T4k

(i)]χδsk (i), i = 1, 2, ..., 34 (78b)

The parameter a = 1 and the scale factor f = 2(a+1). This sets χδsk to give the standard vector of modifiedRodrigues error parameters. Equation (77a) requires that the χδsk (0) be set to zero. This is due to the resetof the attitude error after the update, which is used to move information from one part of the attitude toanother.24 Next the updated quaternions are propagated forward using Eq. (35a), with

˙q(i) =1

2Ξ(q)ωBB/N (i) (79)

with the estimates of the angular rates given by

ωBB/N (i) = [ωBB/I − χbg (i)]−ABN [q(i)]ωNN/I , i = 0, 1, ..., 34 (80)

where χbg is formed from the gyro bias sigma points. The propagated error quaternions are computed using

δq−k+1(i) = q−

k+1(i)⊗ [q−k+1(0)]

−1, i = 0, 2, ..., 34 (81)

Note that δq−k+1(0) is the identity quaternion. The propagated quaternion is computed using23

χδsk+1(0) = 0 (82)

χδsk+1(i) = fδ−

k+1(i)

a+ δq−4k+1

, i = 1, 2, ..., 34 (83)

with δq−k+1(i) =

[

δ−Tk+1(i) δq−4k+1

(i)]T

. With this the predicted covariance can be computed:

q+k+1 = δq+

k+1 ⊗ q−k+1(0) (84)

15 of 20


δq+4k+1=−a‖δs+k+1‖2 + f

√

f2 + (1− a2)‖δs+k+1‖2

f2 + ‖δs+k+1‖2(85)

δ+k+1 = f−1[a+ δq+4k+1

]δs+k+1 (86)

IX. Simulation Results

In this section results are shown for a simulation run that estimates a moving land vehicle’s attitude,position, velocity, rear wheel radii, and biases for the gyro/accelerometer inertial sensors. The total time ofeach simulation is 5 minutes with a measurement update rate of 0.5 seconds. The gyro and accelerometernoise parameters are σgv = 2.9089× 10−7 rad/sec1/2, σgu = 9.1989× 10−7 rad/sec3/2, σgav = 9.8100× 10−5

m/sec3/2 and σau = 6.0000 × 10−5 m/sec5/2 respectfully. The vehicle position is described in geodeticcoordinates, with an initial position located in Buffalo, NY at λ0 = 42.7167 degrees, Φ0 = 78.8667 degreesand height above sea level, h0 = 150 meters. The velocity of the vehicle is given in the NED frame, withinitially the vehicle being at rest. The initial quaternion of the vehicle is found using the attitude matrixfrom the NED to body-frame. The GPS constellation6 uses GPS week 137 at a time of applicability of61440.000 seconds. The number of available satellites is computed using a 15 degree elevation cutoff angle.The filter requires a minimum of 4 satellites with the loose coupling method used in the filter design. Agreater number of satellite signals received results in greater precision of the position measurement. Clock-bias drift is modeled using a random walk process and GPS measurements are found using white-noise errorswith a standard deviation of 5 meters.

The land vehicle follows a track shown in Fig 1. The vehicle begins at rest oriented 60 degrees East ofNorth on a flat surface. For 20 seconds the vehicle accelerates in the heading direction at 1.0 m/sec2. Afteraccelerating the vehicle reaches a velocity of approximately 45 MPH and remains constant for the durationof the simulation. After 0.75 miles the vehicle makes a 90 degree turn, with a roll angle of 1.5 degrees. Thevehicle now travels 1.5 miles and again turns 90 degrees. Next the vehicle continues 1 mile and then pitchesup to 3 degrees for 20 seconds before leveling back off to flat. The vehicle travels along in this manner,pitching down on the opposite side with the same pattern until reaching the origin position. From thisvehicle path information the true states at each time step are found and measured quantities are simulated.

−5000 −4000 −3000 −2000 −1000 0 1000−4000

−2000

0Dow

n(m

)

North (m)East (m)

Figure 1. Path Traveled in Vehicle Simulation

The initial conditions for the filter are chosen to simulate road vehicle initial errors. The initial errorschosen mainly stress the EKF, testing the robustness of the filter design. In the EKF the initial covariancematrix is diagonal, with each component representing the square of the expected value of the error in eachstate. For each filter the initial variance of the attitude is set to a 3σ bound of 15 degrees and initial errorvalues of 7 degrees for the attitude angles. The initial position was set to the true longitude, latitude and

16 of 20


height. The variance for the geodetic angles was set to (1 × 10−6)2 rad2 and the variance for the height is(20/3)2 m2. The initial variance for the velocity for North and East are set to (200/3)2 m2/sec2 and in thedown direction, (10/3)2 m2/sec2. The initial gyro and accelerometer biases are set to zero. The variance ofthe gyro biases are set to a 3σ bound of 30 degrees per hour and the accelerometer biases bounded by 0.005m/sec2. The UF uses parameters of α = 1, β = 2 and κ = −3.

The results of this study compare the use of GPS/INS alone, i.e. the 15-state filter, to the case of GPS/INSwith the wheel speed sensors, i.e. the 17-state filter, using both an EKF and UF estimation algorithm. Thisproduces four sets of estimates which are compared for error convergence and robustness. Each estimationrun is conducted using the same covariance, initial conditions and set of measurements. The results forthe EKF attitude, position and gyro biases are shown in Fig. 2. The attitude results from Fig. 2(a) andFig. 2(b) show the 3σ bounds for the yaw component converges to a steady value within 10 seconds whenthe wheel speed sensors are used, over 20 times faster than when GPS/INS is used alone. For the caseof only GPS/INS the yaw state is the least observable and with the addition of the wheel speed sensorsthis limitation is compensated. Additionally from the EKF attitude results the pitch component convergesslightly faster and the roll is unaffected by the addition of the wheel speed sensors. The UF results for thestates of attitude, position and gyro biases are shown in Fig. 3. The UF attitude filter results from Fig. 3(a)and Fig. 3(b) show the same pattern as the EKF results but with slower convergence overall. In the positionresults for the EKF comparing Fig. 3(a) and Fig. 3(b), shown is slightly faster initial convergence in all stateswith the addition of the wheel speed sensors but convergence to a steady 3σ error bound value in the sameamount of time for both cases. For the case of position the UF produces very similar results to the EKF asshown in Fig. 3(c) and Fig. 3(d).

From the results it seems the EKF provides overall faster convergence as well as having the benefitof requiring less computational power than the UF. This is true for very small initial state errors, butwhen testing for filter robustness the UF can handle far larger errors before the error diverges outside the3σ bounds. When the filters are stressed from increasing initial state errors for the attitude components,especially in yaw, the 15-state EKF could handle much higher initial errors than the 17-state EKF beforethe estimates diverged. Errors as small as 3 degrees in yaw caused the corresponding error to drift beyondthe 3σ bounds. This is seen in Fig. 2(b) for the 17-state EKF yaw component results for when the yawinitial error was set to 7 degrees. For the same initial error values in the 17-state UF from Fig. 3(b) theerrors remain within the 3σ bounds. The UF is more robust in handling these initial attitude errors, withsimilar robustness results in both the 15- and 17-state UF. This indicates that the 17-state UF provides theadvantage of faster convergence from the addition of the wheel speed sensors and does not have the decreasedrobustness as in the 17-state EKF.

X. Conclusion

This paper formulated an estimation algorithm by fusing sensor data from GPS, INS and wheel speedsensors for use in land vehicle navigation applications. Filter results for an EKF and UF were compared forcases of GPS/INS alone and with the addition of wheel speed sensors. Results show faster convergence withthe addition of the wheel speed sensors for yaw, pitch and position in both types of filters. The robustnessof the EKF with the use of the wheel speed sensors was found to be limited for large initial state errors.The UF filter provides increased performance for cases with large initial state errors along with the fasterconvergence from the addition of wheel speed sensors to the GPS/INS at the cost of increased computationaltime.

References

1Jekeli, C., Inertial Navigation Systems with Geodetic Applications, Walter de Gruyter, Berlin, Germany, 2000.2Parkinson, B. W. and Spilker, J. J., The Global Positioning System: Theory and Applications, AIAA, 1996.3Bevly, D. M., “Global Positioning System (GPS): A Low-Cost Velocity Sensor for Correcting Inertial Sensor Errors on

Ground Vehicles,” Journal of Dynamic Systems, Measurement, and Control , Vol. 126, No. 2, June 2004, pp. 255–264.4Abbott, E., Powell, D., Signal, A., and Redmond, W. A., “Land-Vehicle Navigation Using GPS,” Proceedings of the

IEEE , Vol. 87, No. 1, 1999, pp. 145–162.5Bevly, D., Ryu, J., and Gerdes, J. C., “Integrating INS Sensors with GPS Measurements for Continuous Estimation of

Vehicle Sideslip, Roll, and Tire Cornering Stiffness,” IEEE Transactions on Intelligent Transportation Systems, Vol. 7, No. 4,2006, pp. 483–493.

6Crassidis, J. L., “Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation,” IEEE Transactions on

17 of 20


0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

Roll(deg)

Pitch

(deg)

Yaw

(deg)

Time (sec)

(a) 15-State EKF Attitude Errors and 3σ Bounds

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

Roll(deg)

Pitch

(deg)

Yaw

(deg)

Time (sec)

(b) 17 State EKF Attitude Errors and 3σ Bounds

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−20

−10

0

10

20

Lat.

(deg)

Long.(deg)

Height(m

)

Time (sec)

(c) 15-State EKF Position Errors and 3σ Bounds

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−20

−10

0

10

20

Lat.

(deg)

Long.(deg)

Height(m

)

Time (sec)

(d) 17-State EKF Position Errors and 3σ Bounds

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−20

−10

0

10

20

x(deg/hr)

y(deg/hr)

z(deg/hr)

Time (sec)

(e) 15-State EKF Gyro-Bias Errors and 3σ Bounds

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−20

−10

0

10

20

x(deg/hr)

y(deg/hr)

z(deg/hr)

Time (sec)

(f) 17-State EKF Gyro-Bias Errors and 3σ Bounds

Figure 2. EKF Results for Added Wheel Speed Sensor

18 of 20


0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

Roll(deg)

Pitch

(deg)

Yaw

(deg)

Time (sec)

(a) 15-State UF Attitude Errors and 3σ Bounds

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

0 50 100 150 200 250 300−15

−7.5

0

7.5

15

Roll(deg)

Pitch

(deg)

Yaw

(deg)

Time (sec)

(b) 17-State UF Attitude Errors and 3σ Bounds

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−20

−10

0

10

20

Lat.

(deg)

Long.(deg)

Height(m

)

Time (sec)

(c) 15-State UF Position Errors and 3σ Bounds

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−2

−1

0

1

2x 10

−4

0 50 100 150 200 250 300−20

−10

0

10

20

Lat.

(deg)

Long.(deg)

Height(m

)

Time (sec)

(d) 17-State UF Position Errors and 3σ Bounds

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−20

−10

0

10

20

x(deg/hr)

y(deg/hr)

z(deg/hr)

Time (sec)

(e) 15-State UF Gyro-Bias Errors and 3σ Bounds

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−10

−5

0

5

10

0 50 100 150 200 250 300−20

−10

0

10

20

x(deg/hr)

y(deg/hr)

z(deg/hr)

Time (sec)

(f) 17-State UF Gyro-Bias Errors and 3σ Bounds

Figure 3. UF Results for Added Wheel Speed Sensor

19 of 20


Aerospace and Electronic Systems, Vol. 42, No. 2, 2006, pp. 750–756.7Hay, C., “Turn, Turn, Turn Wheel-Speed Dead Reckoning for Vehicle Navigation,” GPS world , Vol. 16, No. 10, 2005,

pp. 37–42.8Kubo, Y., Kindo, T., Ito, A., and Sugimoto, S., “DGPS/INS/Wheel Sensor Integration for High Accuracy Land-Vehicle

Positioning,” Proceedings of the National Technical Meeting of the Institute of Navigation (ION), 1999, pp. 555–564.9Gao, J., Petovello, M. G., and Cannon, M. E., “Development of Precise GPS/INS/Wheel Speed Sensor/Yaw Rate Sensor

Integrated Vehicular Positioning System,” Proceedings of the National Technical Meeting of the Institute of Navigation (ION),Vol. 2, pp. 780–792.

10Saraf, S. and Tomizuka, M., “Slip Angle Estimation for Vehicles on Automated Highways,” Proceedings of the AmericanControl Conference, Vol. 3, 1997, pp. 1588–1592.

11Carlson, C. R., Gerdes, J. C., and Powell, J. D., “Practical Position and Yaw Rate Estimation with GPS and DifferentialWheelspeeds,” Proceedings of AVEC 6th International Symposium, Japan, 2002.

12Farrell, J., Barth, M., Galijan, R., and Sinko, J., “GPS/INS Based Lateral and Longitudinal Control Demonstration:Final Report,” California PATH Research Report, University at California, Berkeley, CA, UCB-ITS-PRR-98-28, 1998.

13Julier, S., Uhlmann, J., and Durrant-Whyte, H. F., “A New Method for the Nonlinear Transformation of Means andCovariances in Filters and Estimators,” IEEE Transactions on Automatic Control , Vol. 45, No. 3, 2000, pp. 477–482.

14Meeus, J., Astronomical Algorithms, Willman-Bell, Inc., Richmond, VA, 2nd ed., 1999.15Decker, B., “World Geodetic System 1984,” Defence Mapping Agency, Aerospace Center, St. Louis, MO, ADA167570,

1986.16Zhu, J., “Conversion of Earth-Centered Earth-Fixed Coordinates to Geodetic Coordinates,” IEEE Transactions on

Aerospace and Electronic Systems, Vol. 30, No. 3, 1994, pp. 957–961.17Shuster, M., “Survey of Attitude Representations,” Journal of the Astronautical Sciences, Vol. 41, 1993, pp. 439–517.18Lefferts, E. J., Markley, F. L., and Shuster, M. D., “Kalman Filtering for Spacecraft Attitude Estimation,” Journal of

Guidance, Control, and Dynamics, Vol. 5, No. 5, Sept.-Oct. 1982, pp. 417–429.19Moler, C. and Van Loan, C., “Nineteen Dubious Ways to Compute the Exponential of a Matrix, Twenty-Five Years

Later,” SIAM review , Vol. 45, No. 1, 2003, pp. 3–49.20Wan, E. A. and Van Der Merwe, R., “The Unscented Kalman Filter,” Kalman filtering and neural networks, 2001,

pp. 221–280.21Bar-Shalom, Y. and Fortmann, T. E., Tracking and Data Association, Academic Press, Boston, MA, 1988.22Bartels, R. H. and Stewart, G. W., “Solution of the Matrix Equation AX+ XB= C [F4],” Communications of the ACM ,

Vol. 15, No. 9, 1972, pp. 820–826.23Crassidis, J. L. and Markley, F. L., “Unscented Filtering for Spacecraft Attitude Estimation,” Journal of Guidance

Control and Dynamics, Vol. 26, No. 4, 2003, pp. 536–542.24Markley, F. L., “Attitude Error Representations for Kalman Filtering,” Journal of Guidance Control and Dynamics,

Vol. 26, No. 2, 2003, pp. 311–317.

20 of 20


diﬀerential wheel speed sensor integration with...

Documents