Note that gyros insensitive to signals equal to or smaller than earth rate are assumed. Vitally important to the successful implementation of inertial positioning.
AUTOMATIC ALIGNMENT AND CALIBRATION OF AN INERTIAL NAVIGATION SYSTEM Samer S. Saab, and Kristjan T. Gunnarsson Union Switch & Signal Pittsburgh, PA 15237
Abstract In this paper we derive a simple six degree of freedom navigator, earth-surface navigator, for terranean vehicle application, using low grade gyros. The calibration and alignment of the navigator are investigated when the system is at rest. Based on the observability of the error model when the system is at rest, a state transformation is presented. This transformation decouples the observable modes, which are based on physical insight, from the unobservable modes. An example is given to illustrate the performance of a Kalman filler for calibration and alignment.
Introduction Historically, inertial navigation systems (INS) have been used in aerospace vehicles [l]: space vehicles, aircraft, missiles, m,uine vessels [Z]: aircraft carriers, ships and submarines, and to a lesser extent land vehicles 131. These applications are Characterizedby the need for accurate navigation over significant trajectories, both in geographic distance and time. The design and analysis of inertial navigation systems for these vehicles must take into account the shape of the earth and its rotation, and gravitational variation with location of the vehicle. The location of these vehicles is normally referenced to geographic frames employing the coordinates of latitude, longitude and height above mean sea level. The resulting INS is of high grade and significant cost. We concern ourselves here with the use of inertial sensing systems to the positioning of terranean vehicles, i.e. vehicles traveling in contact with the surface of the earth. Examples include robotic vehicles, semi- or fully autonomous factory vehicles, and roving site vehicles both in hazardous environments and surveillance applications. These applications are characterized by trajectories over short geographic - - distances; on the order of miles, and small titnc duration; on the order of minutes. The requirements for positioning systems for such vehicles include accurate positioning and low cost. Minimal extemal aiding refercnces are
available. System initialization must be automatic. The trajectories of these vehicles often include frequent stops at known locations and bounded trajectories over semiconstrained roadways. Vehicle position is not specified in terms of geographic coordinates; often the distance travelled. instantaneous speed and grade is all that is required. From the point of view of analysis and design the shape of the earth and its rotation, and gravitational variation with location of the vehicle can be ignored. The design of all inertial sensing systems starts with derivation of the equations of the navigator and its error model. We present a navigator based on the assumptions of a flat earth without rotation and constant gravitational acceleration. The derivation follows Britting, [I]. These assumptions are reasonable with respect to the intended application. Note that gyros insensitive to signals equal to or smaller than earth rate are assumed. Vitally important to the successful implementation of inertial positioning systems is initial, zero velocity, calibration and alignment. This is the topic of the present paper. We define calibration as the estimation of sensor biases and alignment as determining the attitude of the sensing system relative to the positioning frame. In the systems of interest, a cartesian frame of reference is appropriate. This frame has an arbitrary definition of “north”, but “up” is defined with respect to plumb-bob direction of gravity. Because “north is arbitrary, the initial “azimuth orientation of the vehicle either is known. immaterial or must be determined in transit through aiding position references. The analysis of calibration and alignment follows that of Bar-Itzhack and Berman [4]. A Kalman filter [51 is derived to estimate the state of the error model during initialization.
Earth-Surface Frame Navigator The earth-surface frame navigation equations are derived based on the following assumptions:
(1) Neglect the earth angular rate. (2) A constant gravity field applied.
845 0-7803-1435-2/94 $3.00 Q 1994 IEEE
(3) The inertial coordinate frame is chosen as a frame attached at the surface of earth. Also the pointing east-north-up frame is orientation of the earth-surfaceh n e navigator. The following is the dynamics of the earth-surface frame navigator
c:, =
C;*yB
(EQ 3)
where
Note that the eigenvalues of the model describing the INS error propagation are all at the origin; i.e., hi = 0, for i = 1 , 2, ..., 9. It is noted that there is no Schuler frequency associated with this system. Remark: The tilt vector y' (+ Y1 respective antisymmetric matrix) is used to correct the transformation matrix as follows
r
e; 7
= ( I + Y')
e;
(EQ 5 )
and this is only valid for sntall errors in yaw, pitch and roll angles.
IMU Error Model and ab and gbare the accelerometer and gyroscope readings in the body frame respectively. The transformation matrix is initialized using the initial conditions of the roll (O), pitch (0) and yaw (y)angles of IMU (Inertial Measurement Unit) frame with respect to the inertial reference frame, i.e., we rotate around the xaxis ($), then rotate around the y-axis (e), and finally around the z-axis (y).
Earth-Surface f rame Error Mode1 The error model of the earth-surface fmne is given by (EQ. 4), where aR' and aV1 ,are the errors in the radius and its derivative respectively from the origin of inertial frame to the origin of the IMU frame, y' (E913)is a vector due to tilt errors in the navigator, [a'] is a (3x3) inertial acceleration anti-symmetric matrix, a A B and & o , ~ ~are the error vectors in body frame due to accelerometer and gyro errors respectively, and 03 and 13 are the 3x3 zero and identity matrices.
In this paper, we assume that the output of the IMU will contaminated with biased Gaussian noise: i.e.,
) drift vectors. Note where bB and dB ( ~ 3are~constant that the characteristicsof realistic accelerometerand gyro error data can be represented by accurately by linear models driven by white noise processes. Thus, at this point we will assume that qAand qc are zero-mean white Gaussian noise.
th-Surface frame Augnented Error Modd In order to implement a Kalman filter, we augment the system such that the input is zero-mean white Gaussian noise The augmented error model of the earth-surface frame is given
846
.---.-.I..
...
O3 I3
This particular matrix is the actual one when the body is at rest. Consider an error is the yaw, roll and pitch of 1 degree; i.e., a0 = &) = &y = lo, then the anti-symmetric matrix that corresponds to $ can be approximated as follows
O3 O3
O3
o3 o3 - [ d ] c; o3 03 03
4
03
O3
03 03
03
O3 03
o3 o3
o3
03 0 3
for this special case This augmented system has 15 state variables, with anorher 6 eigenvalues at the origin. This is because we have added 6 constant state variables.
Error Analysis The particular solution for the earth-surface frame error model whenever
The above results are in radians which are equivalent to 1 degree (actual transformation matrix is the identity expected tilt!). If we let &(to) = 0, aR)(r,) = 0, then
(1) the inertial acceleration ([a'])is constant (2) the rate in the body (C21~B) is zero
(3) the noise in the IMU is zero
2 2 At At i3 A ~ T~ I ~
3
T ~ La ~ ~ i At
2 At AtCL T A C ~
O3 l 3
ArA
O3 O3
I3
O3
Ate;
O3 O3
O3
I3
O3
03 0 3
03
O3
13
whereAf=r-fu20,and
A
= -[d] x 3
Example: Consider the system is at rest, the body frame is "parallel" to the inertial frame and assume that there is no constant drifts (biases = 0); i.e.,
Thus, in one minute, we have magnitude of the x, y error velocity components equal to 10.2 m/s and 306 m error in the x, y position components. The error due to the accelerometer constant bias contributes to l/g the error due to the tilt angles. The error due to the gyro conslant bias contributes to At/3 the error due to the tilt angles. For example, if II II = II bB II = II dBII, then in 1 minute, the error due the gyro bias will be 20 times bigger than the error due to tilt angles and 196 times bigger than the error due to the accelerometer bias.
w'
847
.
Initial Calibration and Alignment Analvsis In this section the analysis of the calibration and alignment of the earth-surface navigator are investigated when the system is at rest. A similar derivation for a different navigator was carried out in [4]. In most cases, INS alignment and calibration, which are a most essential phase of the operation of any INS, take place when the system is at rest. In this section we are interested only in calibration (observe the IMU bises) and alignment (observe the tilt vector I#). From the tag measurement analysis. it can be observed that the measurement of the velocity or/and measurement of the position vectors contribute identical amount of information to the filter when observing the IMU biases and the tilt vector $. Therefore, for simplicity, we will consider only measurements of the velocity vector. Notice that at rest the ideal velocity vector will be zero at all times.
The same derivation can be applied to the constant drift of the gyro.
The error model (position vector is excluded) is given by
The statement of the problem considered in this section is as follows: Given initial conditions of roll (+), pitch (0) and yaw (w) angles of vehicle with respect to the inertial reference frame (which are assumed to be perturbed), can we observe the corresponding tilt vector and the constant biases of the IMU employing velocity measurements? Unfortunately, the answer is negative. In the following, we study the analysis of observability of the system using control theory approach.
Svstem Dynamics When the system is at rest then the inertial acceleration anti-symmetric matrix [a'] will be given by
(EQ 7)
Lo
OQI
I
I3
O3
CL O 3
O3
4
O3 O3 0
- 3
0
3.
Note that the ideal (excluding the earth rate) gyro readings at rest (in the body frame) are all zero. Therefore, the anti-symmetric matrix form in the body frame is the zero matrix and the transformation matrix is constant. Since we are interested in the observability of this system, we may drop the unbiased noise input. The measurement of the velocity error is considered, where the ideal velocity vector is zero. Thus, the augmented system ut rest can be reduced to (EQ. 14) and the measurement equation is given by
Next, we transfer the IMU constant biases to the inertial frame as follows
(EQ 11)
This model can written as Differentiating the above, we get
z = Hx
(EQ 13)
where the definition and dimension of A, H, x are obvious.
848
-
aGY ai,
-
WX
3, 'z
bX
=
-
0000-g0100000 o o o g 0 0 0 1 0 0 0 0 3% 0 0 0 0 0 0 0 0 1 0 0 0 W, 0000 0 0000100 0000 0 0000010 0 0 0 0 o O O O O O O ~ W, (EQ14)
0000 0 0000000 0000 0 0000000
dX
bx 0 0 0 0 0 0 0 0 0 0 0 0 by 0 0 0 0 0 0 0 0 0 0 0 0 b, 0000 0 0000000 _ o o o o 0 0 0 0 0 0 0 0 _ dx
dY
dY
-4-
-dz-
bz
T =
1000 0100 0010 0000 ooog 0000 0000 0000 0000 0000 0000 0000
0 0 0 -g 0 0 0 0 0 0 0 0
0000000 0000000 0000000 0 100000 0010000 0001000 0000100 0000010 1000000 0100000 0010000
0 0 0 0 0 0 ~
Proof. Since the columns of T are linearly independent, this implies that T is nonsingular. If we define (the similar matrix) L = TAT", and
System Transformation Next, we wish to investigate the observability properties of the above system: i.e., the observability of the system (A, H). In the latter, we introduce a transformation that takes the original state space into a particular state space, where the new state space has a physical interpretation and the new state is decomposed into observable and unobservable state variables. Thus. we can characterize the unobservable space by vectors U of the form U = y g l + bfi2 + bye3 + dgd, where (el, e2, e3, e4} is an orthonormal basis for the unobservable subspace and w,, b,, by and d, are the unobservable state variables where the proof of the last statement is given next.
L augmented
0001000 0 0000100 0 0000010 0 0000000-g
IO000 IO000
oooooog
0 IO000 0000000 0 IO000
L =
0000000 0 10000
- - - - - - - -
+
(EQ 16)
- - - _
0000000 0 IO001 0000000 0 IO000 0000000 0 IO000 0000000 0 IO000 _ooooooo 0 I O O O Q
Theorem 1: Given the pair (A, H) as defined above, define the transformation matrix T as follows then T transforms x into y 0) = Tx) such that (i) y is partitioned into y~ and y2, where y~ is observable and y2 is unobservable.
IO000 IO000
where L above is partitioned and the definition of Lll and L22 are obvious. Thus, y partitioned into yl and y2, where
(ii) the dimension of y l is 8.
849
v(l1)
=
by
(EQ 31)
Y(12)
=
dZ
(EQ 32)
where
It can be readily seen that the system (L,,,H I ) is completely observable (rank of its observability matrix = 8). Therefore. y1 is completely observable and its dimension is 8. Moreover, the system of the state y2 is completely decoupled from y I 'and hence. is not measurable. Therefore, y2 is unobservable. (iii) is obvious from the definition of the proposed transfonnation. Q.E. D. Remark: The transformation defined in this Theorem is not unique. However. the discussion of observability associated with this transformation is related to the physics of the system.
In the following, we consider the transformed state variables given in the above theorem: Observable modes:
Ai)
=
av,
(EQ 21)
y (2)
= al.
(EQ 22)
y(3)
Y
= avz
Again, y(i), fori = 1.2, ..., 8 are the observable modes and fori = 9,10, 11, 12, we have the unobservable modes. The observability of the first 6 state variables is expected from the measurements; i.e., for i = 1,2,3, these modes correspond to the measurement, and for i = 4, 5 , 6 , these ones correspond to the integral of the measurements. Physically, for i = 7,8. at steady state wy = b$g, and = -by/g. In this case, dyy/dt and dy,Jdt have to be zero (since the biases are assumed to be constant). Thus, d, and dy have to be zero. Furthermore, at rest, it will be impossible to observe the yaw angle even in the absence of all biases. Therefore, we should expect not to have a full observation of the tilt angle for alignment when the system is at rest. Note that the estimate of y(6) may be used as a constraint.
wn
Transformation Algorithm Outlines At rest, the system is a linear time invariant. Thus, to apply this transformation scheme, some transformations take place a priori to the filter and others a posteriori. Details are given in the following procedure:
A priori trunsformatioii Systent trarisformalion
(EQ 23) L
Y(5)
=
Y(6) Y (7)
YO)
gw,+by
(EQ 25)
H
= bZ
=
d.x
=
dy
(EQ 26) (EQ 27)
= TAT-]
=
(EQ 33)
T-~H
(EQ 35)
M is the transformed process noise input coupling matrix (only used for the process noise covariance matrix). In this case, the output coupling matrix remains unchanged.
Initial error covariance arid state transformation: (EQ 28)
Unobservable modes:
850
.
...~.,
. "..-
Converpence Characteristics Py (0) = T P x ( 0 ) fl
(EQ 37)
The effect of Py(0) diminishes as the estimation progresses such that in the steady state:
where PJO) and Py(0) are the respective initial error covariance of the state ~(0) and y(0). Py(0) has nonzero offdiagonal elements that introduce correlation between the states. Given time period between samples Tk, then the discretized state matrix is given by (Pk
=
e Tk
(EQ 38)
Process noise covariance matrix Qk transformation Tk T
Qk = j e L t M Q c M T e L 'dt
tEQ 39)
0 where Q, is the covariance matrix of the continuous process noise.
A posteriori transformation
(i) The estimation error standard deviation of the unobservable modes reach nonzero lower bound. (ii) The estimation error standard deviation of the observable modes reach zero. The advantage of the proposed transformation is that it will decouple the unobservable modes from the observable ones. Therefore, it will still be safe to use the closedloop configuration. On the other hand, if the system without the proposed transformation is used, then potential divergence in the closed-loop configuration is expected. This is because the coupling of the unobservable modes. For example, the predicted biases will be off then fed back. and this c,an cause Ihe applied biases to increase in magnitude after every update which in tum initiate a system divergence. Using the proposed transformation, the unobservable modes will converge to some bound and stays there (because of the decoupling of the system). Thus, no divergence should be expected as long as the initial alignment errors are relatively small (refer to the remark in the Earth-Surface frame error model section).
After each state update, the new state y is transformed back to x.
Initial Gvro Calibration
State trangormation
Since the earth-surface frame navigation equations are derived based on neglecting the earth angular rate, then the readings of the gyro at rest will be considered as the corresponding gyro constant drifts. Therefore, we may add to our velocity error measurements the gyro measurements which should be zeros in the body frame and in the inertial frame. Unfortunately, such measurements will not contribute to any additional information to the observation of the tilt angles, hence, they will not affect the system alignment. Thus, the biases of the gyro will be extracted initially before the filter implementation.
IMU biases transformation (Inertial + Body)
What if the gyro is sensitive enough to respond to earth rate? For local application, e.g., say in an area with 400 km radius, then the geodetic latitude angle can be estimated with an error of k.'4 Using the estimates of the IMU orientation, the earth rate contribution to each channel can be estimated and extracted and the residuals may be modelled as random noise.
sB
=
CFb, s E ( b , d )
Feedback the corrected state variables to navigator. Get measurements errors and redo.
851
Numerical ADplications
Conclusions
Data: Sampling period = 1 s. run time = 10 s. measurement error sigma = 1.e-2 m/s (body is at rest), process noise in all the channels of the accelerometer and gyroscope sigma = 1.e-3 m/$ and 1.e-5 ra4s respectively. True yaw. tilt. pitch angles = 0’ and alignment error of 5’ for each angle. Accelerometer constant bias = l.e-3m/$ for each channel. Since we may eliminate the gyro constant drift. thus, in this example no gyro corrstant drifts are considered. Figure 1 shows that the filter is able to eliminate constant accelerometer biases only in the zchannel: the x- and y-channels are left unaltered as they ,are unobservable. Figure 2 shows that the filter is able to eliminate tilt erron. i.e. pitch an roll, but yaw is left untouched as it is unobservable.
An earth-surface navigator and error model based on the assumption of a Bat. non-rotating earth with constant gravitational acceleration was presented. This navigator is to be used in terranean vehicles for short duration trajectories over shon geographic distances. The use of low cost, low grade gyros is assumed. An initial calibration and alignment filter was demonstrated. An observability analysis reveals that the system can be unobservable even for an ideal inertial measuring unit (IMU). A state transformation separating the observable modes from the unobservable ones was shown.
?
G
:
:
=
q
;
-
z
The authors wish to thank L. C. Wang, Penn State University, for early development of the earth-surface navigator and error model.
‘
............................... ..........;..................... . ............ .................................................................... . . ................................................................................ . . . .... .. .. . . ~
AcknowledPmentS
~
~
References
P 5 _.
:
P
.
..
i
..
[ 11 K. R. Britting. Jnertial Navigation Svstems Analvs‘B.
..
Fig. 1 Elimination of constant accelerometer biases through estimation and feedback
Wiley-Interscience,New York, 1971. [2] B. McKelvie and H. Galt, Jr., “The Evolution of the Ship’s Inertial Navigation System for the Fleet Ballistic Missile Program,” Navigation, vol. 25, no. 3, pp. 3 10-322,Fall 1978. [3] J. C. McMillan, “An Integrated Land Navigation System,’’ IEEE Plans 1988, pp. 221-229. [4] I. Y. Bar-Itzhack and N. Berman, “Control Theoretic Approach to Inertial Navigation Systems,” J. Gui& s, vol. 11, no. 3, May-June, 1988, pp. 237-245. [5] A. Gelb, &plied ODtim Est1 MIT Press, Cambridge, MA, 1974.
‘a,
.................................................. . . . 1
2
3
4
5
..........................
........
1
........... .. .......... 9
;
6
7
0
.
.
-...........-. .....
10
1
.
.
: .................. ..
6
7
e
9
io
I
7
I)
9
10
. ........ ;.......... :......................................... t
..
2
3
4
5
2
3
4
5
.
:..........i ........4
1
Fig. 2 Elimination of attitude error through estimation and feedback.
852
