12th International Conference on Information Fusion Seattle, WA, USA, July 6-9, 2009
Improving Self-Alignment of Strapdown INS using Measurement Augmentation Arunasish Acharya School of Education Technology Jadavpur University Kolkata, India.
[email protected]
Smita Sadhu Electrical Engineering Department Jadavpur University Kolkata, India.
[email protected]
Abstract - An alternative solution to the velocity based self-alignment of strapdown inertial navigation system is proposed. It is well known that a simple Kalman filter based solution to the above problem fails to provide accurate azimuth alignment due to the inherent lack of observability of the model in the presence of instrument bias. Earlier researchers use external digital filters to obtain improved estimation of selected states and substitute these into the filter. The current paper demonstrates that a simple augmentation of the output vector with the inertial measurement unit signals and an extended Kalman filter would yield similar or better alignment performance compared to such ad hoc additional digital filters. The proposed method improves the convergence rate of azimuth attitude error even in the presence of gyro bias and makes it relatively independent of the gyro noise. Results of comparative performance of the two filters using Monte Carlo simulation have been provided. Keywords: Self-Alignment, Strapdown Inertial Navigation System, Filtering, Extended Kalman filter.
1
Introduction
This paper addresses self-alignment (that is without external aids) of a strapdown inertial navigation system (INS) [1,2] in near stationary condition, where the knowledge of the local gravity (vertical), latitude and earth’s spin rate are utilized and additional instrumentation is not necessary. Such initial self-alignment in stationary platform is often used for tactical missiles and autonomous ground vehicles. Use of “raw” IMU (inertial measurement unit, comprising of rate gyro and accelerometer) outputs alone can provide only a coarse alignment due to poor signal to noise ratio of IMU’s on stationary platforms [3]. For “fine” alignment, it is customary to use the velocity outputs from the INS (after the due integrations are performed by the navigation software) and from a previously aligned reference INS in a so-named “transfer alignment” [1] configuration, employing Kalman filter (KF) based estimators. For “self-alignment” (that is without the aid of a reference IMU) in stationary
978-0-9824438-0-4 ©2009 ISIF
T.K.Ghoshal Electrical Engineering Department Jadavpur University Kolkata, India.
[email protected]
base and in presence of instrument biases, velocity based scheme produces unacceptable results, especially in correcting azimuth orientation errors due to observability problem [4, 5, 6]. As to be demonstrated subsequently, the poor observability manifests as inaccurate alignment (the azimuth misalignment error being large and roughly proportional to gyro bias) coupled with slow convergence in azimuth. To overcome the observability problem, both [5] and [6] use ad hoc first order digital filters on the gyro measurement signal or on the estimated tilt axes alignment errors respectively. Such filtered output is algebraically combined and substituted in the KF update equations to obtain a more accurate estimate of the azimuth misalignment and a faster convergence. In [6], in particular, the improved estimate of the azimuth misalignment is obtained through approximate algebraic relations involving north axis error and east axis error rate. As the true value of these quantities are unknown, noisy estimates of the azimuth error were filtered and then a KF was applied. The technique used in [5], on the other hand, utilizes filtered gyro measurement to reconstruct estimates about the gyro biases. These values are then substituted to the KF update equations. The present work reports an alternative possibility of using the gyro outputs from the IMU, along with the velocity outputs in a nonlinear state estimation framework, without taking recourse to ad hoc digital filtering and demonstrates that improvement in both convergence rate and residual alignment are possible compared to a (linear) KF implementation. Performance of the proposed scheme has been compared with that obtained in [5]. For a better and more objective comparison, root mean square error (RMSE), its sensitivity to change in instrument bias and measurement error covariances and its convergence rates have been computed for the proposed and earlier scheme [5].
1783
The rest of this paper is organized as follows: the next section presents the truth model, which is followed by a brief KF formulation for the unaugmented system, which has been used as the basis in the two earlier works. This is followed by a performance analysis of the earlier scheme proposed in [5]. Finally, the formulation of the proposed extended Kalman filter (EKF) and augmented measurement based scheme is introduced and the performance of the proposed filter is presented, analysed and compared with earlier scheme [5].
2 2.1
Dynamic models formulation
and
problem
The model for the strapdown INS (SDINS) in the EastNorth-Up (ENU) coordinate system is identical to that used in [5]. It was verified to be consistent with [6], which uses the North-East-Down (NED) axis convention. The East and North axes, in the context of alignment are often called the levelling axes and the “Up” axis is called the azimuth axis. The local gravity vector g and the Earth rate vector ω ie can be expressed as
ω ie = [0 Ω N Ω U ] T where Ω N = Ω cos(L) and Ω U = Ω sin(L) , Ω is earth rate, L is the local latitude, g is magnitude of the local gravity.
(2) with Re =earth radius. The unknown constant biases have been treated as state variables whose time derivatives are treated as zero.
x(k + 1) = Φx(k )
(3)
The constant state matrix is computed numerically using the relation Φ = e AT , at the operating point [7-11], where T=sampling time. Eqns (1) and (3) would also contain process noise terms as the process noise is an artefact which is used by filter designers to take care of model uncertainty including the unknown bias terms.
and
The state equation can be written as
x& = Ax ,
(1)
where, the state vector x = [V E V N φ E φ N φU ∇ E ∇ N ε E ε N ε U ] T, with VN, VE the north and east velocity errors , ΦN, ΦE, ΦU are north, east, up attitude misalignment respectively.
∇E ,∇N
0 −g 0 g 0 0 0 Ω sin(L) − Ω cos(L) − Ω sin(L) 0 0 Ω cos(L) 0 0
The discrete time equivalent of the truth model is given by
The truth model
g = [0 0 -g]T
A 5, 5
0 2Ω sin(L) − 2 Ω sin(L) 0 = 0 0 0 0 0 0
The process noise component for the first five variables may be attributed to small parasitic platform motions due to vibration, etc. As the IMU signals are sampled much faster (compared to the alignment process sampling) for implementing the navigation equation and the IMU signals are filtered in the process, contribution of the IMU noise to the process noise would be small. It is worth noting that the measurements from the INS (as well as from most IMU’s) are generally available at discrete times. The measurement equations with VN, and VE as outputs are written as
are east and north accelerometer biases,
and ε E , ε N , ε U are constant gyro bias (drift rate) of east, north, up gyros respectively, both sets being in navigation frame.
I5 A The state matrix has the form A = 5,5 , where 0 5 is 05 05 a 5 by 5 zero matrix and I 5 is a 5 by 5 unit matrix.
y (k ) = Cx(k ) + v (k ) .
(4)
1 0 0 0 0 0 0 0 0 0 where, C = and v(k) is 0 1 0 0 0 0 0 0 0 0 the measurement noise vector. The measurement error covariance R (k ) is known from the sensor signal processing.
2.2
Gyro output equations
The state equations are expressed in (true) navigation frame. However, if gyro outputs (which are in the body axis) are to
1784
be used as additional measurements, as is done in the proposed method, these quantities must be expressed in the ENU navigation frame. This is done using the appropriate transformation matrix. In body frame, outputs of three gyros are gyro_E= Ω cos(L)(cos( φ N )sin( φU ) + sin( φ N )sin( φ E )cos( φU )) -
Ω sin(L)sin( φ N )cos( φ E ) + ε Eb
(5a)
gyro_N= Ω cos(L)cos( φ E )cos( φU ) +
Ω sin(L)sin( φ E )+ ε Nb
(5b)
gyro_U= Ω cos(L)(sin( φ N )sin( φU ) (5c)
where, ε Eb , ε Nb , ε Ub are constant bias (drift rate) of east, north, up gyros respectively in body frame. Biases in the ENU coordinate system (represented with subscript E) can be transformed into body coordinate system (represented with subscript b) using the following transformation matrix:
C Eb
C11 = C 21 C 31
C12 C 22 C 32
C13 C 23 , C 33
It may be noted that (i) when bias is defined in the navigation (ENU) axis, the state equation is linear but nonlinearity appears in the gyro measurement equations. (ii) Computations for the truth model and filter are performed using radian angular measurements but for specifying the errors, noise covariance and for plotting the alignment errors results, degrees and arc minutes have been used.
2.4
cos( φ N )sin( φ E )cos( φU )) +
Ω sin(L)cos( φ N )cos( φ E ) + ε Ub
(RMS) values of the errors in estimating the misalignment is also determined. The RMSE and the approximate time to reach the steady state estimate are the two metrics to define the quality of alignment. The estimates of the unknown instrument biases have diagnostic value but are not considered as a part of the primary alignment objective.
Parameters used in simulation
As stated earlier, the primary objective of the paper is to improve the alignment performance of the filter compared to earlier reported schemes which were based on adhoc additional digital filters [5, 6]. Hence we assume the values of parameters, initial conditions and noise covariances to be similar to [5] to facilitate comparison. In order to maintain compatibility with [5] to the maximum extent possible, the following parameters, initial conditions and noise covariances have been used: Local latitude L is 45 o, Re =6378100 meter, g= 9.81 m/sec2.
(6) Initial values of velocities = 0.
where, C11= cos(φ N )cos(φU ) - sin(φ N )sin(φ E )sin(φU ) ,
Initial attitude misalignment angles of three gyros are 1o=17.452 mrad.
C12= sin(φ N )sin(φ E )cos(φU ) + cos(φ N )sin(φU ) ,
Constant bias of three gyros are 0.01o / h. Accelerometer constant bias=100µg.
C13= -sin(φ N )cos(φ E ) , C21= -cos(φ E )sin(φU ) ,
Sampling time=1 sec.
C22= cos(φ E )cos(φU ) , C23= sin(φ E ) ,
Process noise covariance matrix:
C31= sin(φ N )cos(φU ) + cos(φ N )sin(φ E )sin(φU ) , C32= sin(φ N )sin(φU ) - cos(φ N )sin(φ E )cos(φU ) , C33= cos(φ N )cos(φ E ) . Note that inclusion of the gyro outputs in the measurement equation makes these nonlinear.
2.3
It has been noted that though a value has been provided for the process noise covariance matrix Q in the earlier reported schemes [5, 6], nothing has been mentioned regarding whether the values are for the continuous form of the matrix or for the discretized form. It has further been noted that the units for the Q matrix mentioned in the papers [5, 6] have obvious errors.
Problem formulation
The self-alignment problem may be formulated as obtaining the estimates of the three angular deviations (that is the 3rd, 4th and the 5th state variables). As the estimates would vary depending on the realization of the process and measurement noise, it is essential that root mean square
Hence, for our analysis, we would assume that the values (with corrected units) provided in [5] are for the discrete model.
1785
Based on the above assumption, the discretized process noise covariance matrix is assumed to be Q(k) = diag ((10µg-sec)2 (10µg-sec)2 (0.005o/h)2(sec)2 (0.005o/h)2(sec)2 (0.005o/h)2(sec)2 0 0 0 0 0 )
RMSE (Degree)
Noise Covariance (0.03 d/h)2
Measurement noise covariance matrix: The velocity measurement error occurs due to imperfections in the implementation of the navigation equations and its value is assigned as 0.0001m/s in each axis. Consequently the covariance matrix of the measurement noise (considering velocity measurements only) is
The fast alignment method of [5] was chosen as a basis for performance comparison and a sensitivity study has been done on the performance results already presented in [5]. The alignment performance in azimuth (RMSE with 200 MC runs) at different values of gyro bias (but nominal noise covariance) is shown in Figure 1. It may be noted that the final error is nearly independent of the gyro bias and that the convergence is reasonably fast, establishing the efficacy of this method. The final error value after 300 seconds is about 0.018 degree.
RMSE (Degree)
0.3 Gyro Bias 0.01 d/h Gyro Bias 0.03 d/h Gyro Bias 0.05 d/h
0.1
0 50
100
150
200
50
100
150
200
250
300
Time (Sec)
Alignment performance using gyro output with digital filter
0
0.1
0
It may be noted that the values of instrument biases and measurement noises quoted above are considered nominal values. During the simulation experiments and sensitivity studies, effect of higher (up to a decade) values of biases and measurement noises have also been explored.
0.2
Noise Covariance (0.05 d/h)2
0.2
0
R(k) = diag ((0.0001m/s)2 (0.0001m/s)2).
3
Noise Covariance (0.01 d/h)2
0.3
250
300
Time (Sec)
Fig. 2 Azimuth RMSE for different gyro noise covariance using Gyro output with Digital Filter The alignment performance in azimuth for different gyro (measurement) noise covariances is shown in Figure 2. It may be noted that the final error (0.075 degree after 300 seconds) and the convergence rate worsens with higher measurement noise covariance.
4 4.1
Alignment using EKF and gyro output augmentation Alignment filter formulation
In this section, a method for alignment using EKF and gyro output augmentation has been proposed. In section 2.1 only horizontal velocity errors were used in the measurement equations. In the present method, attitude misalignment angles along three gyros will also be measured along with the velocity errors. Measurement of attitude misalignment angles along three gyros will affect the state equations. Since output of three gyros are measured in body frame, the drift rates need to be transformed into body frame. So, in state equations εx, εy, εz are to be considered as constant drift error in body frame. Since attitude misalignment angles are very small, it can be considered that sine and cosine of these angles are equal to the attitude misalignment angles and zero respectively. Then it can be easily proved that constant bias measured in body frame is the same as that measured in ENU frame. So, state equations remain unchanged. The measurement equations can be written as
Fig. 1 Azimuth RMSE for different gyro biases using Gyro output with Digital Filter
z (k ) = h( x(k )) + v(k ) , where, z (k ) = [z1 (k )
1786
z 2 (k ) z 3 (k ) z 4 (k ) z 5 (k )]T .
(7)
z1 (k ) and z 2 (k ) correspond to the first two state variables and the last 3 measurements correspond to the discretized form of the gyro outputs provided in Eqns (5a), (5b) and (5c) with additive measurement noises. The measurement error covariance matrix is assumed to be R(k) = diag ((0.0001m/s)2 (0.0001m/s)2 (0.005m/s)2 (0.005m/s)2 (0.005m/s)2)
4.2
EKF and Jacobian
The attitude misalignment angles along three gyros are measured along with the velocity errors and the observation model becomes nonlinear. Since KF can work only on linear systems, EKF is employed here. The state equation is linear whereas the observation equation is nonlinear. Hence, the Jacobian of the observation matrix h is computed and used in EKF.
The alignment performance for the proposed filter in azimuth (RMSE with 200 MC runs) at different values of gyro bias (but nominal noise covariance) is shown in Figure 3. It may be noted that the final error is nearly independent of the gyro bias and that the convergence is reasonably fast, establishing the efficacy of this method. The final error value after 300 seconds is about 0.0133 degree. The alignment performance in azimuth for different gyro (measurement) noise covariance is shown in Figure 4. The final error value after 300 seconds is about 0.0167 degree. It may be noted that the final error is only mildly affected whereas the convergence rate remains more or less the same.
0.3
E 0 2 X 3 H= b S C E
RMSE (Degree)
The Jacobian of h can be written as
(8)
Gyro Bias 0.01 d/h Gyro Bias 0.03 d/h Gyro Bias 0.05 d/h
0.2
0.1
where
1 0 0 0 0 0 0 E= , 0 1 0 0 0 0 0
0
(9)
0
50
100
150
200
250
300
Time (Sec)
S(1, :)=[0, 0, ΩNsin( φ N )cos( φ E )cos( φU ) + ΩUsin( φ N )sin( φ E ), ΩN(-sin( φ N ))sin( φU ) +
Fig. 3 Azimuth RMSE for different gyro biases using EKF and Gyro output Augmentation
ΩNcos( φ N )sin( φ E )cos( φU ) - ΩUcos( φ N )cos( φ E ), ΩN(cos( φ N )cos( φU ) - sin( φ N )sin( φ E )*sin( φU )), 0, 0], (10a)
Noise Covariance (0.005 d/h)2
0.3
ΩNcos( φ E )(-sin( φU )), 0, 0],
RMSE (Degree)
S(2, :)=[0, 0, - ΩN sin( φ E )cos( φU )+ΩUcos( φ E ), 0, (10b)
and S(3, :)=[0, 0, - ΩNcos( φ N )cos( φ E )cos( φU ) ΩUsin( φ N )cos( φ E ),-ΩN cos( φ N )sin( φU ) +
Noise Covariance (0.03 d/h)2 Noise Covariance (0.05 d/h)2 0.2
0.1
ΩNsin( φ N )sin( φ E )cos( φU ) - ΩUsin( φ N )cos( φ E ), ΩNsin( φ N )cos( φU ) + ΩNcos( φ N )sin( φ E )sin( φU ), 0, 0]. (10c)
0 0
50
100
150
200
250
300
Time (Sec)
5
Numerical Simulation and Results
As the performance of the proposed filter in the levelling axes is very similar to the earlier work [5], only the results of azimuth error comparison have been provided in this paper for the sake of brevity.
Fig. 4 Azimuth RMSE for different noise covariance using EKF and Gyro output Augmentation
1787
The alignment performance of the existing and proposed filters have been tabulated in Table 1 where DF indicates the digital filter proposed in [5] and EKF denotes the proposed filter with augmented measurements.
Convergence Time (sec) at 3 arc min
DF
EKF
DF
EKF
Nominal bias and gyro measurement noise cov
1.1
0.8
86
5
Nominal gyro measurement noise and gyro bias five times higher.
1.1
0.8
86
82
Condition
Nominal bias and gyro measurement noise cov hundred times higher.
Conclusions
A new method of self-alignment using gyro output as additional measurement and EKF has been described and its performance has been characterized. One of the reasons for poor performance of the earlier alignment schemes is that such methods are highly sensitive to the measurement noise. Another shortcoming of the previous methods is that both the cited works use filters with large time constants (of the order of 60 s). Any initial condition mismatch may have repercussions on convergence time.
TABLE I Alignment Performance of Filters Final Error (arc min) at 300 sec
6
The proposed method, on the other hand, uses an EKF and augmented measurements. As the residual error of this filter is nearly independent of the instrument bias, there is a possibility of applying this alignment method for fiber optic gyro (FOG) [1] based SDINS. The alignment accuracy will however depend on the noise level. As many such navigation systems are likely to be aided by GPS, once in motion, this method may be employed for an initial alignment exercise.
References [1] A. Bose, S. Puri, P. Banerjee, Modern inertial sensors and systems, Prentice Hall of India, New Delhi, 2008. 4.5
1.0
>300
[2] D. H. Titterton, J. L. Weston, Strapdown inertial navigation technology, Peter Pereguins Ltd., London, 1997.
46
[3] L. Schimelevich and R. Nuor, “New approach to coarse alignment,” IEEE Position Location and Navigation Symposium, Atlanta, GA, USA, 1996. [4] Y. Jiang and Y. Lin, “Error estimation of INS ground alignment through observability analysis,” IEEE Transactions on Aerospace and Electronic Systems, vol. 28, no. 1, pp. 92–96, Jan. 1992.
It is observed that the final errors in the proposed filter are comparable to those for previously published methods and are similarly affected by the gyro bias. The proposed filter exhibits faster convergence irrespective of gyro bias and measurement noise, whereas for previously published methods, the convergence rate deteriorates with measurement noise. The proposed filter is marginally more computation intensive than the earlier methods as it involves 5 outputs instead of 2 and further, Jacobian for the output function has to be evaluated at every iteration. Computation burden may be reduced by (i) considering only the North and Up gyro measurements (ii) by updating the Jacobian (as a background task) only after, say 10 iterations. The former would penalize the final azimuth accuracy and the latter will slow down the convergence.
[5] Z. Chuanbin, T. Weifeng and J. Zhihua, “A novel method improving the alignment accuracy of a strapdown inertial navigation system on a stationary base,” Measurement Science and Technology, vol. 15, pp. 765769, April 2004. [6] J. Fang and D. Wan, “A fast initial alignment method for strapdown inertial navigation system on stationary base,” IEEE Transactions on Aerospace and Electronic Systems, vol. 32, no. 4, pp. 1501-1504, October 1996. [7] D. Simon, Optimal State Estimation, Kalman , H∞ and Nonlinear Approaches; Wiley – Interscience, John Wiley & Sons, Inc., New Jersey 2006.
1788
[8] A.H. Jazwinski, Stochastic Processes and Filtering Theory, New York: Academic Press, 1970. [9] B.D.O. Anderson, J.B. Moore, Optimal Filtering, New Jersey: Prentice-Hall, 1979.
[10] Y. Bar-Shalom and X.R. Li, Estimation and Tracking, Principles, Techniques, and Software, Boston, Artech House, 1993. [11] A. Gelb Applied Optimal Estimation, Cambridge: MIT Press, 1974.
1789
