The local geodetic frame takes basis of making a fictional tangent plane at the origin, just like presenting the globe as a map. The x-axis points north, the y-axis towards east and the z-axis points down, normal onto the ellipsoid, therefore also widely known as the NED-frame (north-east-down). This frame coincides with the geographic frame for a stationary target. The difference between the two is that in the latter frame, the origin is a projection of the platform origin onto the Earth's geodetic. Another version of this frame is the east, north, up-frame (ENU).
The body frame is usually in the center of gravity of the body of the object in question. Its x-axis points towards the defined front of the object, the z-axis points down and the y-axis points right to complete the right hand rule. This frame and the NED-frame are widely used for control purposes.
The frame represents the vehicle states in 6 degrees of freedom (6 DOF) known as surge, sway and heave (u, v, w), and roll, pitch and yaw (φ, λ, ψ). Surge, sway and heave are the speed in x, y and z respectively, and roll, pitch and yaw are the vehicle's angular displacement from the NED-frame.
The transformation from ECEF-to-tangent plane coordinates, starts by subtracting the tangent plane origin, given in the ECEF-frame, from the ECEF coordinates, leaving the two planes with the same origin.
(
x,y,z) (
T x0,y0,z0)
Tx= −
δ (2.1) The next step is performing a rotation around the ECEF z-axis until the y-axis is aligned with tangent-plane east, where the λ is the longitude.
( ) ( )
By performing a new rotation, this time around the aligned y-axis until the new z-axis is aligned with the tangent-plane down where φ is the latitude.
By combining the two, the complete rotation matrix is obtained.
( ) ( ) ( ) ( ) ( )
2.4 Body-to-Tangent Plane Transformation
By using the Euler angles derived from the body frame and transforming via one axis at a time, and by choosing to start with the rotation around the z-axis the new coordinates
[
x′ y′ z′]
Tis obtained.The two remaining axes are applied by the same method, and the body frame coordinates have obtained.
( ) ( )
And these can be combined by multiplication, yielding
( ) ( )
2.5 GPS Coordinate Systems
The use of a reference ellipsoid allows for the conversion of the ECEF (Earth-Centered, Earth-Fixed) coordinates to the more commonly used geodetic-mapping coordinates (LLA) of Latitude(ϕ; in degrees), Longitude (λ; in degrees), and Altitude ( h ；in meters). GPS commonly adopts the LLA coordinate system. The conversion between the two reference coordinate systems can be performed using closed formulas.
The conversion from LLA to ECEF (in meters) is shown as below.
λ ϕcos cos ) (N h
X = + (2.9)
λ ϕsin cos ) (N h
Y = + (2.10)
ϕ sin )
( N h
a
Z = b22 + (2.11)
where N is the radius of curvature (meters), defined as
ϕ
2 2
2 2
1 sin
a b a N a
− −
= (2.12)
a and b are the semi-major and semi-minor axes of the reference ellipsoid of earth, respectively. h denotes the height of the position above the reference ellipsoid. According to the WGS84 parameters, we have
6378137
=
a and 3b =6356752. .
2.5 Local Projection
According to the LLA coordinate system, a small region may be identified with a center for projection (ϕc, λc) onto a flat mapping surface called the x-y plane as follows.
c)
x=κλ(λ −λ (2.13)
c)
y =κφ(ϕ−ϕ (2.14)
The y-axis local project is pointing to the north pole of the earth when the center of projection is on the north semi-hemisphere. The x-axis is determined through the right-hand law with the thumb pointed to the north pole. κλand κϕare the mapping constants from LLA coordinate to local projection, which are defined
κλ = π(N +h)cosϕ/180 (meter/degree) (2.15)
κϕ = 1 2 2 2 2 4 2 2 4 2 180
2 ( ) cos ( )sin )/
( a h b N ϕ a N a Nh ϕ
π a + + −
(meter/degree) (2.16)
Chapter 3 GPS/IMU
3.1 GPS
The GPS is part of a satellite-based navigation system developed by the U.S. Department of Defense under its NAVSTAR satellite program[4].
The fully operational GPS includes 28 or more active satellites approximately uniformly dispersed around six circular orbits with four or more satellites each. The orbits are inclined at an angle of 55o relative to the equator and are separated from each other by multiples of 60o right ascension. The orbits are nongeostationary and approximately circular, with radii of 26,560 km and orbital periods of one-half sidereal day (about 11.967hrs). Theoretically, three or more GPS satellites will always be visible from most points on the earth’s surface, and four or more GPS satellites can be used to determine an observer’s position anywhere on the earth’s surface 24hrs per day.
3.1.1 GPS Signals
Each GPS satellite transmits two spread spectrum, L-band carrier signals – an L1 signal with carrier frequency f1 = 1575.42 MHz and an L2
signal with carrier frequency f2 = 1227.6 MHz. These two frequencies are integral multiples f1 = 1540 f0 and f2 = 1200 f0 of the base frequency f0 = 1.023 MHz. The L1 signal from each satellite uses binary phase-shift
keying(BPSK), modulated by two pseudorandom noise(PRN) codes in phase quadrature, designated as the C/A-code and P-code. The L2 signal from each satellite is BPSK modulated by only the P-code. Both frequencies are available for all users, but due to encryption of the P-code, only the C/A-code is usable by the public.
3.1.2 Sources of Errors
Civilian GPS receivers have potential position errors due to the result of the accumulated errors[5] due primarily to some of the following sources:
1. Ionosphere and troposphere delays:
The satellite signal slow as it passes through the atmosphere. The system uses a built-in model that calculate an average, but not an exact, amount of delay.
2. Signal multi-path:
When the GPS signal is reflected off object such as tall buildings or large rock surfaces before it reaches the receiver.
3. Receiver clock errors:
Since it’s not practical to have an atomic clock in your GPS receiver, the built-in clock can have very slight timing errors.
4. Orbital errors:
The more satellites the receiver can “see”, the better the accuracy.
Anything can block signal reception, causing position errors or possibly no position reading at all, like figure 3.1.
6. Satellite geometry/shading:
Ideal satellite geometry exists when the satellites are located at wide angles relative to each other. Poor geometry results when the satellites are located in a line or in a tight grouping.
7. Intentional degradation of satellite signal:
The U.S. military’s intentional degradation of the signal is known as
“Selective Availability”(SA) and is intended to prevent military adversaries from using the highly accurate GPS signals. SA accounts for the majority of the error in the range. SA was turned off May 2, 2000, and is currently not active. This means we can expect typical GPS accuracies in the range of 6-12 meters.
3.1.3 Velocity Measurements
The integration is performed in two different frames, depending on the measurement. For position, ECEF geodetic [φ, λ, h] is used, while for speed, the tangent-plane (NED) has been chosen. In order to transform the GPS data, the position, already given in ECEF geodetic coordinates, is differentiated. To transform the speed, yielding that:
( ) ( )
3.2 Inertial Measurement Unit(IMU)
The IMU chosen is the MicroStrain@ 3DM-GX1(Figure 3.2). It consists of three accelerometers, three gyros and three magnetometers, and giving acceleration in 6 degrees of freedom(6 DOF) and position in 3 DOF. This IMU performs an accelerometer bias stability of 0.01G, where the G is the Earth’s gravitational constant, and 0.7o/sec for the gyros. And the sensors have bandwidth of 100hz, and transmit data over an RS-232 serial line.
3.3 GPS Module
The GPS module chosen is GARMIN GPS-18-PC(Figure 3.3). This module operates at 8 – 30V, and transmits data through RS-232. It has position accuracy at 15m, or 3m with Wide Area Augmentation System(WAAS) enabled. And its update rate is 1Hz, so it can transmit 1 record per second.
Chapter 4 Kalman Filter
The Kalman Filter is an optimal, linear state estimator, able to estimate the full system state, depending on incomplete and noisy measurement series. The theory of the filter dates back to 1960, when Rudolf Kalman proposed the Filter to NASA for the Apollo Program. The Kalman filter is essentially a set of mathematical equations that implement a predictor-corrector type estimator that is optimal in the sense that it minimizes the estimated error covariance—when some presumed conditions are met. Since the time of its introduction, the Kalman Filter has been the subject of extensive research and application, particularly in the area of autonomous or assisted navigation. The filter comes in many different forms, but the one most relevant for this work is the discrete Kalman Filter, which also will be the one most thoroughly investigated.
4.1 Conventional Kalman Filter Implementation
The Kalman filter addresses the general problem of trying to estimate the state x∈Rn of a discrete-time controlled process that is governed by the linear stochastic difference equation. The basic operation of Kalman Filter is shown in Figure 4.1 and 4.2.
The a priori estimation:
1
1 −
−
− = k + k
k Axˆ w
xˆ (4.1) The a posteriori estimation:
) v xˆ H z ( K xˆ
xˆk = k− + k − k− + k (4.2) The random variables w andk v represent the process and k measurement noise, respectively. They are assumed to be independent of each other, white, and with normal probability distributions
) Q , (
~ ) w
( N 0
p
) R , (
~ ) v
( N 0
p
In practice, the process noise covariance Q and measurement noise covariance P matrices might change with each time step or measurement. The Kalman gain K is obtained from the updating scheme:
−1
−
− +
= H (H H R)
Kk Pk T Pk T (4.3) Where
Q A )
KH I
(
A − +
= −−
− T
k
k P
P 1 (4.4) Problem of the conventional Kalman Filter based position estimation is that the update frequency is determined by when the sensor input z k frequency. If the sampling frequency of z is low, for example sampling k frequency is 1Hz for the GPS sensor, the position error can be as large as 100 m for a vehicle running at speed of 100km/hr.
4.2 Model reference Kalman Filter Estimation
combine the GPS and IMU with different data rate. The basic operation of Model reference Kalman Filter is shown in Figure 4.3.
Discrete form:
k k
k (I B)Ax Bz
x = − −1 + (4.5) Where B is diagonal matrix and the diagonal entries can only be Boolean (0 or 1). The a posteriori estimation:
)
The sampling rate of IMU is higher than that of the GPS in practice.
Assuming that the sampling time of IMU is T, the system matrix of the plant may be written as follows.
⎥⎥
The states are
⎥⎥
The sensor matrix compares only the position differences as follows.
⎥⎦
Matrix B may be written into the following form.
⎥⎦
The Kalman gain K is obtained from the updating scheme:
−1
−
− +
= H (H H R)
Kk Pk T Pk T
4.2.1 Effect of Covariances
The update scheme of Kalman gain is
−1
Assuming that the priori error covariance Pk−and Kalman gain take the following forms in the steady state.
⎥⎦
order polynomial equation (4.6). k1 (position Kalman gain) may be obtained from the following relation (Appendix C).
η
2 2
1 1 k
k = − (4.7) The Kalman gains are function of the ratio of the position covariance and velocity covariance, η, not the independent values. That is for the systems with the same ratio η, the Kalman gains are identical, which
may be considered as the water bed effect.
4.2.2 Adaptive Filtering with Fading Factor
The Kalman gain K is obtained from the updating scheme:
−1
Since the diagonal entry of matrix B is a function of sensor data status, the updating scheme of Pk− may then be simplified into
dmax denotes the maximum lost of GPS data endurable to the system. d denotes the current loss of GPS data count. The equivalent covariance
'
Q of the process noise is monotonically decreasing with d. 10<α ≤ denotes the weight corresponds to the loss GPS data rate, the equivalent
count is based on a given sampling time T . The situation when the loss data count d >dmax implies that the GPS is down or the vehicle is coming into a full stop thus the position cannot be further updated by the GPS. In such case, one must terminate the position estimation process and set xk =HTzk.
4.2.3. GPS/IMU Data Fusion
The GPS data is not only useful for updating the position but also the velocity component in zk . A simple forward difference method, known as the linear approximation, may be used for the velocity approximation purpose as shown in Figure 4.4. Also the circular approximation based three known local coordinates may also be used to perform the précised estimation especially when the vehicle is moving on the circular track.
The other information may be obtained from the linear and circular approximation are the information of the yaw angle of the vehicle which may be simply written as follows.
) tan(x
y
GPS &
&
θ =
Since the GPS data refreshing rate is lower than the IMU refreshing rate, 1 to 7 in our study, the IMU accelerometer may be used to update the velocity estimated by GPS. The IMU gyro yields the yaw angle θgyro of the vehicle can be used to determine the direction of the velocity. When
the new velocity is obtained, we will set δ in matrix B to be 1.
When the vehicle is running on the circular path, the insufficient sampling rate of GPS will cause the position component of zk be zero for a long time, the position estimation of xˆk relies entirely on the velocity component of zk. However, the direction of the velocity must according to the IMU updates. Figure 4.5 shows that the process of GPS and IMU data fusion. In case of high speed turning, the IMU is not fast enough to perform the immediately update, the vehicle will then tend to outrun the circular path. Since The IMU gyro also yields the yaw angular rate θ&gyro of the vehicle, a yaw compensation scheme proposed according to the yaw angular rate as follows may be useful to prevent the outrun situation.
Chapter 5 Experiment and Comparisons
5.1 Test Track Coordinate System
The benchmark is based on the raw DGPS data acquired from ATRC high speed test track, as shown in Figure 5.1, consists of three oval shape runways. The total length of the runway is around 3575 m including 2000 m straight way and 1575m circular way (radius of the middle runway is about 250m). The DGPS data is received on a car which runs in a nearly constant speed, say 100 km/hr, on the runway. The sampling time of DGPS is 1 second and the sampling time of the IMU is 140 ms. The geodetic reference (datum) used for GPS is the World Geodetic System 1984 (WGS84) including the latitude and longitude of the geographic coordinates. Equivalently, the rate of IMU data is 3.89m per data received and the rate of DGPS data is 28m per data received when the car speed is 100km/hr which is nearly the resolution of DPGS used in this work.
Knowing the symmetry the oval shape test track, we are able to estimate the center coordinate of the test road by summing all the DGPS data and then dividing the total coordinate value by the number of data taken, i.e.
∑
==
= n
j j
c n 1
1 λ
λ
λ =24.06764624
∑
==
= n
j j
c n 1
1 ϕ
ϕ
ϕ =120.382617
The corresponding ECEF of the center point of the track is calculated
based on eq.(2.9) and (2.10) as follows.
Xc= -2947234.9 Yc = 5026933.9 Zc = 2585262.1
Substituting the center point into the local project coefficient indicated in eq. (2.13) and (2.14), we have
κλ = 101698.51 κϕ= 110760.24
According to eq. (2.13) and (2.14), we obtain that xj = 101698.51(λj −λc)
yj =110760.24(ϕj −ϕc)
An oval shape track with known dimension consists of 4 segments which are 2 line segments, as shown in Figure 5.2,
Segment #1: x−xo,1 + ytanθ =0 (5.1) Segment #2: (x−xo,2)2 +(y− yo,2)2 −R2 =0 (5.2) Segment #3: x−xo,3 + ytanθ =0 (5.3) Segment #4: (x−xo,4)2 +(y− yo,4)2 −R2 =0 (5.4) where xo and xo are the linear offsets of the line segment #1 and #3
Since the line segments are constrained by the shape of the track, we have The centers of the circular segments are also constrained by the shape of the track as follows.
θ) tanθ Substituting eq. (5.5), (5.6) and (5.7) into eq.(2.9) to (2.12), there are totally five independent unknown parameters in eq. (2.9) to, (2.12), which may be integrated into a vector form as follows.
[
xo,1 R yo,2 yo,4]
TU = θ
One may write the four segment equations as follows.
1 0
In order to linearize the above equations, we let U
According to the Taylor expansion,
= 0
The above equation yields that ) It is obtained that
[
1 2 0 0 0]
D U
CΔ = (5.14) MatrixCand vectorD may be assembled as follows.
⎥⎥
The least square solution yields that D The iteration among procedures according to eq. (5.15) , eq. (5.12), and
U
U~ = ends when ΔU ≤ε and the Hessian condition is satisfied for the minimum, we obtain U~
U≈ . According to the least square result of U~
, the test track coordinate system, as shown in Figure 5.3, including x , c y , L , R , and c θ may be obtained that
2
4
2 ,
, ~
~o o c
x
x x +
=
2
4
2 ,
, ~
~o o c
y
y y +
=
The nominal test track may be written into a formula as follows in terms of the parameter u .
) , , , , , (
g x y L R u y
x
c
c θ
⎥ =
⎦
⎢ ⎤
⎣
⎡
5.2 Spiral Curves and DGPS Inaccuarcy
The difference U~)
, j(
fi between the DGPS data and the least square test track may be calculated. The residue of vector D may form a trajectory variance that
D DT μ =
The trajectory variance μ contains not only the DGPS inaccuracy but also the spiral curves. Spiral curves are generally used to provide a gradual change in curvature from a straight section of road to a curved section. They assist the driver by providing a natural path to follow.
Spiral curves also improve the appearance of circular curves by reducing the break in alignment perceived by drivers[2]. Figure 5.4 shows the
map the local position (x , j y ) onto the test track parameter j u that j minimize the offset from the test track, i.e.
)
The trajectory error for local position may be written as )
It may be possible to use the cumulative moving average to calculate the spiral curves as follows.
1
The maximum DGPS inaccuracy is then evaluated as follows.
)
Since the particular type of the DGPS we used in this study is with 3 meter inaccuracy, we will choose p such that eGPS = 3m.
5.3 Experiments and Analysis
In order to increase the accuracy, there are 3 methods we can adjust.
Following the process below and input these values, there will be a best final result.
1. Adjusting the Q/R ratio.
2. Finding the best Fading factor.
3. GPS/IMU data fusion and Yaw compensation.
There are 3 output values that we can evaluate the results. The Position-variance is the error of position after our process. The Velocity-variance is the error of velocity after our process. The Overall-variance is equal to var
(
POS)
2 +var(
VEL)
2 . And the values of these variances are the smaller the better.5.3.1 Adjusting the Q/R ratio
In Kalman Filter, the purpose of the weights is that values with better estimated uncertainty are “trusted” more. The Q denotes process noise covariance(Velocity-covariance), and the R denotes measurement noise covariance(Position-covariance). In this Model reference Kalman Filter(MRKF), the smaller covariance means that we “trust” it more. The Q/R ratio is equal to
) ( covariance Position
) ( covariance Velocity
R Q
−
− . Figure 5.6 shows that
the Q/R ratio getting bigger will causes the var(POS) getting smaller but it also causes the var(VEL) getting bigger. In our process, the velocity of vehicle is about 3m per data and the accuracy of DGPS is about 3m, so the ratio between R and Q will be the best result at about 1:1.
shading of buildings. In order to keep MRKF working on the situation of GPS loss data count, we should input the adaptive Fading factor.
Increasing Fading factor means the covariance Q of process noise is monotonically decreasing. In other words, because of the GPS loss data count, we trust the measurement process less and estimation process will be trusted more. Figure 5.7 shows that the bigger Fading factor will increase the Position accuracy but decrease the Velocity accuracy, and the best Fading factor is about 0.5.
5.3.3 GPS/IMU data fusion and Yaw compensation
The GPS data is not only updating the position but also the velocity component in z .When we use GPS data to update the velocity k component in z , the velocity accuracy will be dramatically increased. In k our application, the vehicle is not only moving on the straight line but also on the circular track. In order to know the direction of velocity, we have to use the IMU. The accelerometer of IMU may be used to update the velocity estimated by GPS and the gyro of IMU yields the yaw angle θgyro of the vehicle can be used to determine the direction of the velocity.
As the time goes on, the gyro will yield some drifts and cause the error of yaw angular rate. In order to correct the effect of drift, we should add the yaw compensation. Figure 5.8 shows the GPS/IMU data fusion and the yaw compensation will increase the velocity accuracy but not causing the
As the time goes on, the gyro will yield some drifts and cause the error of yaw angular rate. In order to correct the effect of drift, we should add the yaw compensation. Figure 5.8 shows the GPS/IMU data fusion and the yaw compensation will increase the velocity accuracy but not causing the