A Stationary North-Finding Scheme for an Azimuth Rotational IMU Utilizing a Linear State Equality Constraint

The Kalman filter (KF) has always been used to improve north-finding performance under practical conditions. By analyzing the characteristics of the azimuth rotational inertial measurement unit (ARIMU) on a stationary base, a linear state equality constraint for the conventional KF used in the fine north-finding filtering phase is derived. Then, a constrained KF using the state equality constraint is proposed and studied in depth. Estimation behaviors of the concerned navigation errors when implementing the conventional KF scheme and the constrained KF scheme during stationary north-finding are investigated analytically by the stochastic observability approach, which can provide explicit formulations of the navigation errors with influencing variables. Finally, multiple practical experimental tests at a fixed position are done on a postulate system to compare the stationary north-finding performance of the two filtering schemes. In conclusion, this study has successfully extended the utilization of the stochastic observability approach for analytic descriptions of estimation behaviors of the concerned navigation errors, and the constrained KF scheme has demonstrated its superiority over the conventional KF scheme for ARIMU stationary north-finding both theoretically and practically.

Generally, the single-axis rotation approach applies the azimuth rotational motion periodically to the IMU. Different schemes can be implemented for single-axis rotation, such as flip/dwell schemes [3,4], continuously rotating schemes [8,11,13], reciprocating rotation schemes [9,10], and so on. In the subsequent sections of this paper, an IMU with the continuously rotating scheme is called the azimuth rotational IMU (ARIMU). As it behaves with some specific characteristics, comprehensive research on the ARIMU is of great significance. From a mathematical point of view, Britting proved that continuous rotation offers an advantage in attenuating the effects of time-correlated random drift change and the operational environment is significantly better for the gyro during rotation [8]. An experimental method based on the fast orthogonal search for a practical observation model to separate angle random walk error from the RLG measured data on a turntable with continuous rotation was proposed in [11]. Theoretical explanations of the principle of restraining navigation errors by continuous rotation based on the navigation error equation were presented in [13].
In many practical applications, the azimuth determination requirements are high accuracy and short time [5][6][7][8][14][15][16][17]. The Kalman filter (KF) has always been used to provide self-calibration and to improve azimuth performance under practical conditions [2,3,6,[8][9][10]]. An observation model that includes north and east velocities and geographic frame east rate measurements was presented and illustrated by simulated IMU date in [16]. To improve self-alignment scheme for a strapdown INS in near stationary conditions, a nonlinear augmented measurement-based observation model was proposed in [18]. A rigorous analytical method of incorporating state equality constraints in the Kalman filter was developed to improve the prediction accuracy of the filter in [19].These literatures are all very instructive to analysts concerned with azimuth determination problems.
Although a considerable amount of research on determining the azimuth angle has been done for the ARIMU [3][4][5][6][7][8][9][10]13], they did not pay much attention to the estimation behaviors (e.g., convergence rate) of navigation errors when implementing a Kalman filter. Taking advantage of its basic properties of intuitive linear-algebraic characterizations of the stochastic observability, azimuth behaviors during in-flight alignment when several characteristic maneuvers are performed were investigated and explicit findings were obtained analytically with simple models in [14,15].
The major thrusts of this paper are to provide a novel rapid north-finding filtering scheme for the ARIMU on a stationary base using a state equality constraint and to extend the utilization of the stochastic observability approach for analytic descriptions of estimation behaviors of the concerned navigation errors. For illustrative purposes, a postulate system, the principal idea of which is close to that of [9,10], was taken as the experimental set-up.
The remainder of the paper is organized as follows: in Section 2, an error dynamic model for the ARIMU and a conventional observation model for stationary north-finding are presented, followed by formulation of a linear state equality constraint and a proposed constrained KF for stationary north-finding. Section 3 describes the analytical solution for characterizing estimation behaviors of the concerned navigation errors during stationary north-finding based on the stochastic observability approach. Results and discussions of experimental verification are given in Section 4. Conclusions are drawn in Section 5.

Modeling Error Dynamics for the ARIMU
In this subsection, a commonly used navigation error dynamic model is presented. For the reduction of computation load when applying stochastic observability analysis, the error dynamic equations in the ψ formulation will be introduced in this study. Figure 1 depicts the experimental set-up and a diagrammatic sketch of the coordinate frames in the ARIMU. The IMU contains a three-axis ring laser gyro (RLG) triad, a three-axis vibrating quartz accelerometer triad and their associated electronics. We denote by b the IMU fixed frame, and the zb axis lies along the turntable shaft center and downwards vertically to the turntable plane. As the IMU is rigidly fixed on a continuously rotational turntable, the b frame is rotated with the turntable rotation [11]. For the sake of convenience, we define the b0 frame when the xb axis coincides with the turntable null indicator. Then, we have: where α represents the rotation angle of the xb axis with respect to the turntable null indicator, and . Ω denotes the turntable rotation rate.
As the b0 frame is turntable fixed, 0 n b C remains constant on stationary base. In practice, the b0 frame is approximately leveled, the roll and pitch angles are small enough, it yields: 0 cos sin sin cos sin cos cos sin where θ, γ, φ denote the roll, pitch and azimuth angles, respectively. The objective of north-finding is to determine φ in this study. Thus, a transformation from the b frame to the n frame may be expressed as the product of Equations (1) and (2), we may write: Neglecting other error sources such as factor error, temperature effect, and so on, gyro triad biases and accelerometer triad biases are assumed to be constant. Then, basic navigation error equations in ψ form expressed in the north-east-down (NED) coordinate system are as follows [16,20,21]: where i, e, n, represent the inertial frame, the Earth frame, and the local geographic navigation frame, respectively. ψ represents the attitude error vector, and ψ = [ψN ψE ψD] T . δv represents the velocity error vector expressed in the n frame. n ie ω is the turn rate of the Earth frame with respect to the i frame expressed in the n frame, and n en ω is the turn rate of the n frame with respect to the e frame. f represents a measure of the specific force acceleration. ε and ∇ are defined as gyro triad bias vector and accelerometer triad bias vector, respectively.
The error items including attitude error components of ψ, north velocity error δvN, east velocity error δvE, gyro bias components of ε b , accelerometer bias acting about the xb axis b x ∇ , and accelerometer bias acting about the yb axis b y ∇ are taken into account in the state update equations of the implemented Kalman filters. Meanwhile, n en ω is equal to 0 when the b0 frame is at rest. Hence, error dynamic model for the ARIMU is given by: Components of wf add small random walk quantities to δv and ψ. The matrix F takes the following form: Similarly, we have: For stationary north-finding, the position has been initialized and the ARIMU uses zero velocity updates, vN and vE are supposed to be zero [20,22]. Therefore, δvN and δvE are chosen as the measured quantities conventionally. The conventional observation model is given by: where I2×2 is a second order identity matrix. v(t) denotes the measurement noise vector. Main noise sources of the measurement noise are vibratory motions and small position displacements due to human activity, such as disturbance, loading, fuelling, and boarding. Sometimes, the measurement noise covariance is artificially increased to prevent instability of the Kalman filter [16,22]. In addition, it is assumed that w(t) and v(t) are uncorrelated [20,22]. Then, optimal estimation of the navigation error states can be achieved based on the augmented error dynamic model (8) and the conventional observation model (11) using a Kalman filter, which we called the conventional KF scheme.

The State Equality Constraint Formulation and the Proposed constrained KF Equations
By analyzing the characteristics of the ARIMU on a stationary base, we obtain a linear state equality constraint to modify the conventional observation model in this subsection. Theoretically, angular rate measured by the gyro triad under stationary conditions can be expressed in the b frame, that is: However, as mentioned before, gyroscopes are unfortunately subject to errors such as constant bias, bias stability, temperature effect, and so on. Considering only the gyro constant bias exists as in Equation (6) in the true environment, angular rate measured by the gyro triad under stationary condition, denoted by b ib  ω , is given by: (13) During stationary north-finding, the estimated direction cosine matrix n b  C is used for attitude update computation. Therefore, computed angular rate in the realized computer program can be written as: where ( ) (12)-(14), we obtain: Ignoring small quantity product terms, we then have a state equality constraint equation: As can be seen in Equation (16), both of E ψ and b z ε are navigation error states of the error dynamic model (8). Therefore, we can incorporate the linear state equality constraint (16) in the conventional observation model (11). Then, the modified observation model may be expressed as: where wzb represents the computed angular rate measurement noise acting about the zb axis. Using gyro triad measured data and turntable rotation information, the added linear measured quantity b ib z δω can be computed to fuse the effects of the two error states into one. Meanwhile, it should be noted that, although they are neglected above in the basic navigation error equations for simplification, error sources whose influencing effects are equivalent to the gyro bias b z ε have been taken into account by the added measured quantity b ib z δω and will be estimated in the filtering process. Taking gyro triad factor errors for example, their influencing effects in the gyro triad measured data equals the gyro triad biases when the turntable rotation rate is constant and the ARIMU is stationary, so the estimated value b z ε will eventually include their equivalent component on the zb axis gyro [22].
Consequently, we propose a constrained KF using the augmented error dynamic model (8) and the modified observation model (17), which is called the constrained KF scheme in the following sections.

Stochastic Observability Measures
The stochastic observability approach, which connects the observability analysis with investigation of convergence rates of the concerned navigation errors when implementing a Kalman filter, has demonstrated its effectiveness in successful applications to solving in-flight alignment problems in [14,15]. With the stochastic observability approach, we will provide in this section an analytical solution for characterizing estimation behaviors of the navigation errors during stationary north-finding. Meanwhile, estimation performances of the concerned navigation errors utilizing the conventional KF scheme and the constrained KF scheme are compared. The following analytic and numerical computations are carried out with Wolfram Mathematica 7.0 tool.

Stochastic Observability
Before entering stochastic observability analysis, some necessary explanations and definitions of observability of linear systems adopted in this study are introduced. For purpose of extending known definitions to stationary north-finding problem, we will adopt the following meanings.
In the absence of process noise and a priori information, the solution to the continuous system matrix Riccati equation in P −1 is given by [14,23]: where Ф(t, τ) represents the transition matrix corresponding to A, and the integral on the right-side of Equation (18) is the stochastic observability matrix of the linear system. Based on characteristics of the state transition matrix [23], we have: Along with the use of the stochastic observability approach, special attention should be paid to the following points: (a) According to the precondition of Equation (18), it should be noted that stochastic observability analyses in the following subsections are performed without considerations of process noise and a priori knowledge of the initial states. (b) The system is uniformly completely observable when the stochastic observability matrix is positive definite and bounded for some t > 0 [23].
As indicated in Equation (18), information (decrease the estimation error variance) about states that are initially completely unknown may be acquired through processing measurements. Besides, with error covariance matrix computations, convergence rates can be analytically investigated to maintain good physical insight into the estimation behaviors of the navigation errors [14,23].

Analytic Descriptions of Estimation Behaviors of Navigation Errors during North-Finding
To characterize estimation behaviors of the concerned navigation errors clearly, acquiring analytic relationships between the navigation errors and correlated influencing variables are the most essential solutions, as is one of our main purposes in this study. In this subsection, we focus on the azimuth error and the zb axis gyro error, which are the two most crucial state variables in the error dynamic model (8). The diagonal elements of the covariance matrix P(t) indicate estimates of the magnitudes of the navigation errors. Thus, the third and eighth diagonal elements of the covariance matrix P(t), which are denoted as P(3,3)(t) and P (8,8)(t), may be regarded as the quantitative measures of observability of the azimuth error and the zb axis gyro error [8,[14][15][16]20,21]. Suppose that total time for stationary north-finding is ts, we have P(ts) for the conventional KF scheme from Equation (18): H H (20) where variances of v1(t) and v2(t) are assumed to be equal to the same value, and the value of diagonal elements of R are denote by r0 2 . Similarly, P(ts) for the constrained KF scheme is given by: where variance of zb w , which is equal to the value of the third diagonal element of R , is denoted by rg 2 .
As numerical values of the diagonal elements of the error covariance matrix P at time ts implies ultimate estimation accuracies of the navigation errors, we can investigate improvement of the constrained KF scheme with respect to the conventional KF scheme by computing P(ts). Due to the complex products of trigonometric function in n b C computation, complete symbolic computation on P(ts) still require computer with rather high performance. Therefore, it is difficult to derive general conditions for the analytic solution. To circumvent this problem of acquiring analytic descriptions of estimation behaviors of the navigation errors during stationary north-finding, we compute P(ts) with certain influencing variable analytically to reduce computation load.
In the simplified P(ts) calculation, the WGS-84 model is applied, and the total time ts is 600 s. The b0 frame is supposed to be coincidence with the n frame. Besides, it is assumed that the stationary north-finding be carried out at a latitude at sea level of 0.492535 rad. Then, there remains only the measurement noise standard deviations r0 and rg and the turntable rotation rate Ω as the influencing variables.

The Effects of the Measurement Noises on Final Azimuth Error and Final zb Axis Gyro Error
To investigate the effects of the measurement noises on final azimuth error and final zb axis gyro error at 600 s analytically, the turntable rotation rate must be determined. For consistency with the subsequent experimental verification, Ω is set to be 0.6981 rad/s (or 40 deg/s).
Combining Equations (11), (19) and (20), one can obtain the numerical value of ( ) 0 s t P . Then, analytic description of final azimuth error with r0 for the conventional KF scheme worked out to be: And, analytic description of final zb axis gyro error with r0 for the conventional KF scheme is given by: Similarly, one can obtain the numerical value of ( ) 1 s t P by combining Equations (17), (19) and (21). Then, the analytical description of the variance of final azimuth error with r0 and rg for the constrained KF scheme may be expressed as follows: The analytical description of the variance of final zb axis gyro error with r0 and rg for the constrained KF scheme can be obtained as: To compare the estimation behaviors of the concerned navigation errors by the above two filtering schemes, covariance calculations can be accomplished numerically based on Equations (22)-(25) to obtain the necessary information.   Figure 2, it is clearly seen that 1-σ values of final azimuth error and final zb axis gyro error are theoretically directly proportional to r0 when Ω is set.
1-σ values of the final azimuth error and final zb axis gyro error at 600 s computed according to r0 and rg when using the constrained KF scheme are shown in Figure 3. The numerical range of r0 is the same as in the conventional KF scheme, and the numerical range of rg is 2 × 10 −5 deg/h to 0.2 deg/h. In Figure 3 it can be seen that the state equality constraint adopted in the constrained KF scheme has a great influence on the estimation behaviors of the concerned navigation errors. Obviously, the constrained KF scheme has achieved much better north-finding performance than the conventional KF scheme. The 1-σ value of the final azimuth error is theoretically proportional to r0 when Ω and rg are set. However, the 1-σ value of the final azimuth error behaves with a nonlinear variation with rg when Ω and r0 are set.

The Effects of the Turntable Rotation Rate on Final Azimuth Error and Final zb Axis Gyro Error
The measurement noise standard deviation r0 will be determined likewise in this subsection for practical implementation. From an empirical standpoint, r0 is set to be 0.01 m/s on a stationary base.
Analytical descriptions of the final azimuth error and final zb axis gyro error at 600 s with Ω for the conventional KF scheme take a rather complex form including the items Ω, trigonometric functions with 600 Ω, and so on. Therefore, we can not obtain simple expressions for ( ) ( )  In Figure 4, it is illustrated that the final azimuth error and final zb axis gyro at 600 s error approach a steady value when the turntable rotation rate is high enough, which indicates that increasing the turntable rotation rate has little influence on improvement of the estimation accuracy of the concerned navigation errors at this moment.   Figure 5 has given a more explicit indication, which yields that the final zb axis gyro error behaves with an oscillatory convergence with turntable rotation rate increases. However, it can be seen that final azimuth error approaches a steady value with little fluctuation when the turntable rotation rate increases.
Similarly, diagrammatic representations of the 1-σ values of the final azimuth error and final zb axis gyro error at 600 s by error covariance computations according to different turntable rotation rates and rg for the constrained KF scheme are shown in Figure 6, respectively.
In Figure 6, it can be seen that the final azimuth error and final zb axis gyro error have been decreased greatly even when rg approaches 0.2 deg/h. Meanwhile, the final azimuth error and final zb axis gyro error approach a steady value when the turntable rotation rate is high enough, and also the turntable rotation rate threshold is the same as in the conventional KF scheme. 1-σ values of the final azimuth error and final zb axis gyro error at 600 s computed according to turntable rotation rates ranging from 0.0003 rad/s to 0.1 rad/s for the constrained KF scheme when rg is 0.2 deg/h are shown in Figure 7, respectively. Compared to Figure 5, it can be seen in Figure 7 that the constrained KF scheme has improved the estimation performance of the azimuth error greatly. However, it should be noted that oscillatory convergence characteristics of final zb axis gyro error with turntable rotation rate increase still exists. Therefore, analysts dealing with ARIMU at low turntable rotation rates should pay attention to this phenomenon and need to split the difference between final azimuth error and final zb axis gyro error, which is similar to the idea expressed in [24].

Convergence Rates of the Azimuth Error and the zb Axis Gyro Error
By setting r0 and Ω to be 0.01 m/s and 0.6981 rad/s respectively, the convergence rates of the azimuth error and the zb axis gyro error during stationary north-finding by the two filtering schemes within total time ts are investigated in this subsection. Figure 8 gives convergence curves of the azimuth error and the zb axis gyro error with error covariance computations by Equation (20)   Convergence curves of the azimuth error and the zb axis gyro error with error covariance computations by Equation (21) within 600 s for the constrained KF scheme are shown in Figure 9, respectively. It should be noted that the final zb axis gyro error at 600 s is directly proportional to rg. Therefore, the measurement noise wzb should be restrained by appropriate techniques to improve the north-finding performance. From Figure 9, it can be seen that the azimuth error and the zb axis gyro error converge with time faster when rg approaches the smaller value.
Compared to Figure 8, it is seen that convergence rates of the azimuth error and the zb axis gyro error by the constrained KF scheme are much faster than by the conventional KF scheme, even when rg is 0.2 deg/h. Meanwhile, from the error covariance computation results, the final azimuth error at 600 s can theoretically be reduced to just a few arc-seconds by the constrained KF scheme.
As illustrated above, extended application of the stochastic observability approach proposed in [14,15] for analytical description of a higher order error model has been accomplished successfully in this study. Ultimately, the above analytical depictions and graphic presentations of the stochastic observability analysis have provided important information showing that the constrained KF scheme displays a great estimation performance improvement over the conventional KF scheme.

Experimental Results of the Postulate System
The first performance evaluation of the rate-biased RLG in the postulate system, which utilizes the concept of the ARIMU, was given in [11]. The system has been developed to meet rapid north-finding challenges. Steps actually required to accomplish a rotation scheme of the postulate system are listed below: (a) It can be seen in Equation (17) that, the measurement noise wzb existing in the added measured quantity b ib z δω is composed of gyro triad measurement random errors and turntable rotation random errors. Therefore, a 2 milliseconds sample time of the IMU measured data and turntable rotation information and an angular resolution of 0.18" of the turntable photoelectric angle encoder are implemented on the experimental platform, as can be confirmed to restrain the measurement noise wzb to low enough [11].
(b) According to the estimation results of the average angle random walk of the constant rate-biased RLG in the postulate system presented in [11], the turntable rotation rate should be larger than 40 deg/s to obviate the effect of lock-in at the low rotation rates. Besides, it helps little to improve stationary azimuth performance when the turntable rotation rate is higher than 0.1 rad/s. Thus, the turntable is determined to be continuously rotating at a rate of 40 deg/s.

Theoretical Analysis with Practical Considerations
As indicated earlier, stochastic observability analysis of the above two filtering schemes in Section 3 were carried out without consideration of process noise and a priori knowledge of the initial states. In practical applications, there is always a coarse azimuth determination phase before the fine north-finding filtering phase to provide the a priori knowledge of the initial states to match the applicability conditions of Equations (4) and (5). After the coarse azimuth determination phase, the 1-σ value of the azimuth error worked out to be 1.64°; 1-σ values of the roll and pitch angle errors are equal to the same value, which is 0.1°, assuming that both of the 1-σ values of the north and east velocity errors are 1 m/s. In the laboratory environment, the measurement noise standard deviation r0 is 0.01 m/s, and rg is 0.002 deg/h. The a priori knowledge of the initial states is given by Therefore, convergence rates of the azimuth error and the zb axis gyro error in the true environment can be obtained by error covariance computations with Equation (27) in a similar manner as in Section 3. With the a priori knowledge of the initial states, convergence curves of the azimuth error and the zb axis gyro error within 600 s for the conventional KF scheme and the constrained KF scheme in the practical application are shown in Figures 10 and 11, respectively. Compared to Figure 8, it is obvious in Figure 10 that the zb axis gyro error actually converges slowly at the beginning until the azimuth error has achieved a certain estimation accuracy in the practical application. From Figure 11, it is seen again that the zb axis gyro error converges much faster with the constrained KF scheme than by the conventional KF scheme. However, combining Figures 10  and 11, one can see that estimation of the azimuth error by the constrained KF scheme does not show better performance than by the conventional KF scheme at the beginning in practical applications. Compared to the estimation accuracy achieved by the conventional KF scheme, the better estimation of the azimuth error at the end of the filtering process by the constrained KF scheme has benefited from the better estimation of the zb axis gyro error. Looking through Figures 8-11, it can be seen that, to a certain extent, influences of P(0) on the azimuth error and the zb axis gyro error will converge to zero with filtering carrying on.

Experimental Results
Then, multiple practical experimental tests can be done on this postulate system at a fixed position to compare the stationary north-finding performance possible with the above two filtering schemes.
Convergence curves of the azimuth error and the zb axis gyro error within 600 s north-finding time by the conventional KF scheme are shown in Figure 12 Convergence curves of the azimuth error and the zb axis gyro error within 600 s north-finding time obtained with the constrained KF scheme are shown in Figure 13, respectively. Apparently, as the added measurement quantity has improved the degree of observability of the zb axis gyro bias, the azimuth error and the zb axis gyro error have converged to steady value much faster by the constrained KF scheme than by the conventional KF scheme. Additionally, the 1-σ values of final azimuth error and final zb axis gyro error at 600 s are 4.4 arc-seconds and 0.0014 deg/h from the statistical calculation, respectively.
From Figures 12 and 13, it can be seen that the constrained KF scheme behaves better in north-finding performance than the conventional KF scheme. However, compared to results of the stochastic observability analysis with the a priori knowledge of the initial states, there is still potential to improve the estimation accuracy of zb axis gyro bias.

Conclusions
To improve the north-finding performance of the ARIMU on a stationary base, a constrained KF using a state equality constraint has been studied in depth. By analyzing the characteristics of the ARIMU on a stationary base, we obtained a linear state equality constraint to modify the conventional observation model. Taking advantage of its basic properties of intuitive linear-algebraic characterizations of the stochastic observability, analytical representations of estimation behaviors of the concerned navigation errors when implementing the conventional KF scheme and the constrained KF scheme during stationary north-finding were derived. Evidently, explicit formulations of navigation errors with influencing variables have helped us to maintain good physical insights into the estimation behaviors of the concerned navigation errors during stationary north-finding when implementing the schemes, which is one of the most significant advantages of the stochastic observability approach.
Multiple practical experimental tests at a fixed position were done on a postulate system to compare the stationary north-finding performance by the two filtering schemes. The experimental results show that the azimuth error and the zb axis gyro error have converged to a steady value much faster by the constrained KF scheme than by the conventional KF scheme. From our theoretical investigation and practical experimental verification of the ARIMU with ring laser gyros, the constrained KF scheme has on the whole demonstrated its superiority over the conventional KF scheme for the purposes of ARIMU stationary north-finding.