Professional Documents
Culture Documents
I z ux ,CG
Cf Cr
u x ,CG +
Cr b Cf a
mu x , CG
Cf a 2 Cr b 2
I z u x , CG
) u
C
mf
+ C a (1)
r Izf
y ,CG
K = P (t )C T CP (t )C T + R
or K = P+ (t )C T R 1
(4)
P+ (t ) = [I KC ]P (t ) or P+ (t ) = P (t ) + C R C
1
where:
x (t ) = prior estimate of the system state at time t
x+ (t ) = updated estimate of the system state at time t
P (t ) = prior error covariance matrix at time t
P+ ( t ) = updated error covariance matrix state at time t
K = Kalman Gain
y (t ) = new measurement
C = observation matrix
R = measurement noise covariance
P (t + 1) = Ad P+ (t ) Ad T + Q
(5)
where:
Ad = discretized system dynamics matrix
= I + t A (using Euler method)
Q = process noise covariance
0 0 0 a x ,bias 0 0 a x , m
x ,bias = 0
+
u& y , sensor r 0 0 1 u y , sensor 0 1 a y ,m (13)
0 0 0 a x ,bias 0 0
a& y ,bias 0
where:
mGPS = yaw angle measurement from GPS
GPS
u GPS
cos( GPS )
x ,m = U
(11)
GPS
u GPS
sin( GPS )
y ,m = U
(12)
u GPS
y , m = u y , sensor + noise
where:
u GPS
x ,m = longitudinal velocity measuremen t from GPS
u GPS
y ,m = lateral velocity measuremen t from GPS
+ noise
(9)
where:
r = & = rm rbias = compensated yaw rate
u y ,m 0 0 1 0 u y , sensor
a x ,bias
(14)
(15)
where:
r = road grade estimate
U V ,U H = vertical and horizontal velocity from GPS
Figure 4 shows the comparison of longitudinal
accelerometer bias and grade estimate using filtered
GPS velocities.
(17)
where:
p = roll rate
ha = distance from roll center to GPS antenna
hs = distance from roll center to INS sensor
bias
where:
pm , pbias = roll rate gyro measuremen t and bias
Fig. 6 Roll Center Model with Grade and
Superelevation
A roll center vehicle model with road grade and
superelevation is shown in Figure 6. This model
assumes the vehicle body rotates about a fixed point
(the roll center) on a frame that remains in the plane of
the road. Since the grade of the surface can be estimated
from the GPS velocity measurement and vehicle roll
including the superelevation of the surface can be
measured utilizing the two-antenna GPS receiver, the
expected gravity component in the acceleration
GPS
m
= t + noise
(19)
where:
mGPS = roll angle measurement from GPS
where:
I xx , b, k = roll moment of inertia, damping ratio, and stiffness
m = vehicle mass
h = distance between roll center and center of mass