1. INTRODUCTION
Inertial Navigation System (INS) is a type of dead-reckoning navigation system and must to be initialized prior to its operation (Pitman, Reference Pitman1962). In many practical applications, the higher accuracy Master INS (MINS) is available to aid the alignment of the Slave INS (SINS). This process is appropriately known as Transfer Alignment (TA), and its significance has been recognized in both theoretical research and in modern navigation applications.
The past few decades have witnessed a marked rise in TA techniques to meet the increasing demands of quick reaction performance in weapons systems. Because weak observability is commonly considered the best starting point to lengthen the required time in the aligning process (Groves, Reference Groves2003), most previous literatures endeavoured to increase the observability magnitude of the misaligned states. Bar-Itzhack proposed a framework of observability analysis of time-varying system in (Meskin and Itzhack, Reference Meskin and Itzhack1992a; Reference Meskin and Itzhack1992b). Within this framework, the manoeuvring-related observability during the in-flight alignment was investigated in (Itzhack and Porat, Reference Itzhack and Porat1980), (Porat and Itzhack, Reference Porat and Itzhack1981), and (Meskin and Itzhack, 1992). Rhee expanded the instantaneous observability results to the integrated INS/Global Positioning System (GPS) system in (Rhee et al., Reference Rhee, Abdel and Speyer2004). In the presence of time correlated noise, Wendel presented a rapid TA scheme (Wendel et al., Reference Wendel, Jurgen and Gert2004) where the full observability was achievable with brief Wing Rock (WR) manoeuvres. In order to excite azimuth-related states to extreme magnitude, Efraim proposed a new in-drilling alignment procedure (Efraim and Mintchev, Reference Efraim and Mintchev2007a; Reference Efraim and Mintchev2007b). Furthermore, TA experiments were designed in various practical platforms, such as F-16 fighters (Kevin and William, Reference Kevin and William1998) and other air-to-surface weapons (Ross and Elbert, Reference Ross and Elbert1994). More recently, diverse observability analysis results were summarized and the general analysis tools were given (Lee et al., Reference Lee, Lee, Koh, Park, Moon and Hong2010; Hong et al., Reference Hong, Chun, Kwon and Lee2008). Based on these literatures, since more latent states can be excited by vehicle motion, artificial manoeuvres are necessary to drive the alignment model to a uniformly observable one. However, as pointed out in Groves (Reference Groves2003), due to the existence of lever-arm, the manoeuvre motion can result in greater measurement uncertainty accompanied by increased flexure and vibration, which reduces the observability of INS error-states. In addition, challenges arise and the manoeuvres may be very difficult or even impossible to implement because of the huge inertia of warships in shipborne applications.
Motivated by the above observations, this paper presents a novel framework to achieve fast alignment performance with reduced-manoeuvre (or even zero-manoeuvre) demands. Under this framework, the traditional TA model is equally described as a definitely observable system subject to a group of state-constraints, which is the so-called Constrained Transfer Alignment (CTA) model. The corresponding constrained estimation problem can be solved by employing some newly presented filters as surveyed by Simon (Reference Simon2010).
The remainder of this paper is organized as follows. In Section 2, the traditional TA problem is formulated. Based on this, the CTA model is derived and analysed in Section 3, where the convergent performance of the Moving Horizon Estimator (MHE) in the CTA model is also proved. Finally, simulation results with different manoeuvres and concluding remarks are presented in Section 4 and Section 5, respectively.
2. PROBLEM FORMULATIONS
2.1 Glossary of Terms
n = Local navigation frame of axes
b=Real body frame of axes
b* = Computed body frame of axes
Cab = Direction cosine transformation between a-frame and b-frame
v = Velocity, m/s
a = Acceleration, m/s2
ψ = Attitude angle
∇ = Instrument error vector of accelerometers
ε = Instrument error vector of gyros
Δx = Variable displacement vector
2.2. Formulations
In Kain and Cloutier (Reference Kain and Cloutier1989), the framework of TA via velocity matching is presented by the following differential equations:
where:
Δv is velocity difference between the master and slave velocities.
Δψ denotes the ‘misalignment’ of the slave INS b*-frame from the ‘true’ North-East-Down (n-frame).
Cb*n is the estimated body-to-navigation frame direction cosine transformation.
afbb denotes the flexible body acceleration which is equal described by a stochastic process.
∇ab and ε gb are parameterized instrument error vectors of accelerometer and gyros, respectively.
ω fbb is the flexible body rotation rate.
As pointed out (Britting, Reference Britting1971; Itzhack and Porat, Reference Itzhack and Porat1980; Yi, Reference Yi1987 and Wendel et al., Reference Wendel, Jurgen and Gert2004), the accelerometer and gyro instrument error parameterization is very complex for accurate modelling. However, most literatures consider the random error of gyros as a mixture of three independent elements that will be referred to hereafter as successive start drifting ε bi, random walk ε ri and white noise signals w g. Consequently, ε bi and ε ri can be formulated as:
where τ g means the correlated time of gyros' output.
The total error of gyro is:
The instrument error of accelerometer ∇ is modelled as a mixture of random constant ∇b and white noise w a:
where $$\dot \nabla _{bi} = 0,i = x,y,z$$.
Generally speaking, the commonly selected states include ∇v, ∇Ψ, εgi and ∇i, i=x, y, z. It is worth noting that some additional states can also be augmented into other TA models. However, for simplicity, those states are not involved in this paper.
Let be the lever-arm vector denoting the relative distance between the SINS with respect to the MINS. Then the relationship between velocity of the MINS and the SINS can be shown (Sun and Deng, Reference Sun and Deng2009), as:
where r + δr.
The lever-arm compensation on the velocity output of the MINS can be calculated as follows:
Then the measurement equation for velocity matching case in traditional TA takes the following form:
where:
$$\upsilon _v = C_a^n {\hskip4pt\bf \dot \tilde\hskip-2pt r\hskip1pt}^a $$ and can be seen as white measurement noise.
δ r represents the lever-arm error vector and can be modelled as random constants.
Then, the traditional TA model consists of Equations (1)–(6) and Equation (9). Based on this traditional formulation, the objective of this paper is to obtain an equal TA framework with uniform observability.
3. MAIN RESULTS
3.1 Constrained Descriptions of Instrumental Errors and Lever-Arm
Without loss of generality, the random walk component in Equation (4) is widely accepted as a first-order Gauss-Markov process. Suppose the inertial outputs are sampled with unit interval, the correspondingly discrete model can be written as:
The correlated time of gyros τ g is generally longer than several hundreds of seconds. Substituting Equations (10)–(13) into Equation (5) yields:
Similarly, the accelerometer error in Equation (6) can be rewritten as:
It is worth noting that the above instrumental errors are represented in the slave body frame. Because the established instrumental error model varies with inertial instruments and modelling methods, for simplicity, we only consider a well-accepted instrumental error model here. However, it is easy to extend this constrained description framework to other error models.
On the other hand, the effects of lever-arm motion and ship flexure remarkably confine the alignment accuracy and rapidity in the presence of ship-dynamics, which are rather difficult to compensate due to their strong uncertainties (Titterton and Farnborough, Reference Titterton and Farnborough1990). In this section, we suggest that the lever-arm can also provide auxiliary information to evaluate the initial misalignment-states. As depicted in Figure 1, the coupled relationships of relative velocity and angular rotation between the MINS and SINS in the presence of dynamic deformation can be equally described as a group of constraints.
The relationship between relative linear velocity and angular velocity is well known as:
where ω sn can be arranged as:
Equations (14), (15) and (17) constitute the constrained descriptions of instrumental errors and lever-arm effect. The CTA model can be summarized as the following discrete-time form:
subject to:
where:
3.2. Observability Analysis on TA Models
It is worth noting that the observability analysis of dynamic time-varying systems is still an open question. On the basis of observability analysing methods (Ham and Brown, Reference Ham and Brown1983; Meskin and Itzhack, Reference Meskin and Itzhack1992a; Reference Meskin and Itzhack1992b), the observability analyses on traditional TA models were referenced in many previous literatures (e.g., Jiang and Lin, Reference Jiang and Lin1992; Fang and Wan, Reference Fang and Wan1996). Generally speaking, the velocity and angular misalignment can be easily observed under proper measurement model, and it is hard to observe all the instrumental error uniformly at the same time. In this section, the observability analysis is introduced in the proposed CTA system.
Meskin and Itzhack (Reference Meskin and Itzhack1992a; Reference Meskin and Itzhack1992b) have pointed out that the observability analysis of the time-varying alignment system at the jth segment, after having gone through segments 1, 2,…, j − 1, has to be carried out on $${\bf \tilde Q}_j $$, the Total Observability Matrix (TOM) at that segment. The matrix $${\bf \tilde Q}_j $$ is constructed as follows:
where:
$${\bf \tilde Q}_i^T = \left[ {\matrix{ {{\bf H}_i^T |({\bf H}_i {\bf A}_i )^T |({\bf H}_i {\bf A}_i^2 )^T |\ldots|({\bf H}_i {\bf A}_i^{n - 1} )^T} \cr}} \right],1 \leqslant i \leqslant j$$, can be seen as the discriminant matrix of observability according to the classic control theory in the instantaneous case.
Δi is the time span of segment i.
Furthermore, the TA models constitute dynamic systems for which Theorem 2 in (Meskin and Itzhack, Reference Meskin and Itzhack1992a; Reference Meskin and Itzhack1992b) holds, thus we can use the Stripped Observability Matrix (SOM) $${\bf \tilde Q}_s (\,j)$$ for simplicity. The SOM is constructed as:
For the constrained TA model in Equations (18)–(22), the instantaneous discriminant matrix in segment i is:
where:
Please refer to (Britting, Reference Britting1971) for details on the above notations. We can easily find that the rank of the SOM is uniformly equal to six even without any manoeuvring motion, which equally means that the constrained system is completely observable in every segment. See (Rao et al., Reference Rao, Rawlings and Mayne2003) for detailed definitions of observability.
3.3 Moving Horizon Estimator and its Modified Version in CTA Case
When it comes to the constrained estimation problem, various filters other than traditional Kalman filtering have been presented (Simon, Reference Simon2010), such as projecting methods, unscented filtering and truncated particle filtering, etc. To maintain a trade-off between accuracy and calculating costs, the MHE (Muske and Rawlings, Reference Muske and Rawlings1993), which stems from the Bayesian Maximum a Posterior (MAP), is modified to better suit the proposed framework. The Bayesian MAP estimation of x given y essentially means the most likely value of x, given y is:
Essentially speaking, MHE is an approximation of MAP estimation with a moving, fixed-size estimation window. The fixed-size estimation window is necessary to bound the size of the quadratic program. Most previous works assume that the noise among the system is mutually independent and the initial state and noise have (truncated) Gaussian distributions with zero means, where the posterior probability can be easily derived (Muske and Rawlings, Reference Muske and Rawlings1993; Goodwin and Hernan, Reference Goodwin and Hernan2004), while in the CTA case as proposed in Equations (18) and (19), we take the varying means Gaussian distributions into account. That is:
where R, Q and P 0 are the covariance matrix of measurement noise, system noise and error covariance of initial states, respectively.
Using Bayes' rule, we have (Rao, Reference Rao2000)
Furthermore:
where the minimization is done subject to the dynamics and all constraints. $$|x||_Q^2 = x^T Qx$$, w −1 = 0 and the ‘cost function’ is defined as:
where:
Now we use a moving window (horizon) of length N, so the cost function can be rearranged as:
Define the arrival cost of a state z∈R T − N at time T − N as:
R T − N denotes the reachable set of the state space subject to all constraints and dynamics of the system. Then the optimization problem of the proposed CTA model can be rearranged as:
subject to Equations (20)–(22), and the optimal solution of Equation (31) is the MHE.
3.4 Stability Analysis of MHE in CTA Case
3.4.1 Definition 1
Definition 1 states “A function f:Rn → Rn is a K-function if it is continuous, strictly monotone increasing, f(x)>0 for x≠0, f(0) = 0, and $$\mathop {\lim} \limits_{x \to \infty} f\,(x) = \infty $$.”
Suppose f(.) is a K-function, then its inverse function f− 1 is also a K-function. Actually, it is easy to prove that the space of K-functions is closed under addition, composition and positive scalar multiplication.
3.4.2. Definition 2
Definition 2 states “An estimator is an asymptotically stable observer for the system
if, for every feasible initial condition x 0, and for every ϵ > 0, there correspondingly exists δ>0 and a positive integer such that if $|x_0 - \hat x_0 || \leqslant \delta $, then $|x(T;x_0, 0) - \hat x_T || \leqslant {\rm \epsilon} $for all $T \geqslant \bar T$. Furthermore, for all feasible x 0, $\hat x_T \to x(T;x_0, 0)$as T → ∞.”
3.4.3. Theorem 1
In the CTA case as described in Equations (18)–(22), for all $\hat x_0 \in {\rm {\opf X}}_0 $, the MHE is an asymptotically stable observer as defined in Definition 2.
3.4.4. Proof
The stability analysis procedure consists of the following three steps:
3.4.4.1. Step 1
The convergence of $\{{\rm \hat \Phi} _k \} $: monotone non-decreasing and bounded above sequence is convergent.
Suppose the sequence $\{{\rm \hat \Phi} _k \} $ is the moving horizon solution of Equation (29), we have:
From the definition of arrival cost as described in Equation (30), we can easily find that:
Suppose N → ∞, the global optimization of Equation (28) is determined as:
Because of the existence of the above infinite sum, the partial sum of $\Phi _T^ *$ is limited by a certain bound, denoted as a constant C here, then:
Thus we have proved the sequence $\{{\rm \hat \Phi} _k \} $ is monotone non-decreasing and upper bounded. Hence, it is convergent, and the partial sum of $\{{\rm \hat \Phi} _T \} $ tend to zero:
3.4.4.2. Step 2
∀ K-function θ(.), $\forall {\epsilon} $, ∃ζ such that if $\Vert x\Vert \leqslant \zeta $, then θ(x) ≤ ɛ. Thus for the convergence of sequence $\{{\rm \hat \Phi} _k \} $, ∃N 0, if T⩾N 0, $\sum\limits_{k = T - N}^{T - 1} {L_k (\hat w,\hat \upsilon )} \leqslant \zeta $ hold, then $\forall {\epsilon} $, $\theta \left(\left\Vert\sum\limits_{k = T - N}^{T - 1} {L_k (\hat w,\hat \upsilon )}\right\Vert\right) \leqslant {\epsilon} $ is guaranteed.
3.4.4.3. Step 3
In this step we prove that there exists a positive integer that for all i > , there correspondingly exists a K-function θ(.) that guarantees $\vert x_i - \hat x_i \Vert \leqslant \theta \left(\left\Vert\sum\limits_{k = T - N}^{T - 1} {L_k (\hat w,\hat \upsilon )} \right\Vert\right)$.
Based on the aforementioned observability analysis, we can easily found that the CTA model is uniformly observable, which guarantees that there exists a positive integer N 1 and a K-function φ(.) such that for any two states x 1 and x 2 (Rao et al., Reference Rao, Rawlings and Mayne2003):
where: for all k⩾0.
Specially, assume k = 0, then:
According to the Cauchy-Schwarz inequality, the square form of Equation (39) can be rearranged as:
The relationship between system outputs according to different initial states can be arranged as:
From the CTA model, we can easily establish that the group of functions f i(x)=k ix (k i∈{HAj,HAj− 1G,…,HG}) are all Lipschitz continuous functions. Correspondingly, there exists a group of constants c i∈ R+(i = 1, 2,…,j + 1), named as the Lipschitz constants of f i(.), such that:
Using the Cauchy-Schwarz inequality repeatedly, Equation (40) can be rewritten as:
Suppose $\{ (x_0 - \hat x_0 ),\hat w_0,\ldots, \hat w_{\,j - 1}, \hat \upsilon _j \} $ are not simultaneously equal to zero, and the bounded-ness feature of the practical noise sequence {w 0,…,w j−1, υ j} is taken into account, then we can infer that there exists C 1=max{c 12,c 22,…,c j + 12,1} and C 2∈R, such that:
Furthermore, the partial sum of the above inequality satisfies:
where $L_k (\hat w,\hat \upsilon ) = \Vert \hat w_k - \hat w_{k - 1} \Vert _{Q^{ - 1}} ^2 + \Vert \hat \upsilon _k\Vert _{R_0^{ - 1}} ^2 $.
Thus, from Equation (39), we have:
where K 1=C 1.C 2(Q+R+P 0), K 2=N 1(N 1 + 1)K 1 and K 3=N 1K 2.
By recalling the properties of K-function given before, we assume that there exists a function θ− 1(.), defined as $\theta ^{ - 1} (||x_0 - \hat x_0 ||) = [\varphi (||x_0 - \hat x_0 ||)]^2+ r.||x_0 - \hat x_0 ||_{P_0^{ - 1}} ^2,$$ \forall r \in {\bf R}$, which also satisfies Definition 1. This equally means that there exists K-function θ(.) such that:
Analogously, we can prove that Equation (47) still satisfies for ∀i ⩾ 0.
Retrospectively, from the conclusion achieved in Step 2, we can conclude that $\forall {\epsilon} \gt 0$, ∃N 0, if i ⩾ N 0−N holds, we have:
This completes the proof.
4. SIMULATIONS
In this section, an illustrative example is given to demonstrate the effectiveness and applicability of the proposed methods.
The traditional TA model with velocity matching is formulated as Equations (1)–(9), and the proposed TA model is described in Equations (18)–(22). According to the performance index of commonly adopted inertial instruments, the simulation conditions are selected as:
ε bi = 1(deg/h).
∇b = 10 μg.
Q = 10−10diag (1, 1, 1, 100, 100, 100).
R = 10−6diag (1, 1, 1).
τ g = 300 (s).
The initial misalignments of SINS are assumed as:
Δψ = (10, 10, 10)T (deg).
Δv = (1, 1, 1)T (m/s).
It is worth noting that here we consider the constraints in Equation (29) as linear constraints according to the fact that if $x\sim {\bf N}(\bar x,\sigma ^2 )$ such that $p[ - 3\sigma \leqslant$$ (x - \bar x) \leqslant 3\sigma ] = 99\!\cdot\! 7\% $. The length of moving window is selected as N = 10 and the manoeuvre schemes are designed according to the feasibility in warship cases.
4.1. Scenario 1: Uniform Turn Motion
In this scenario, the vehicle is assumed in uniform turn motion with approximate constant velocity and angular rotation. Here we suppose that the angular velocity in azimuth direction is ω z = 1(°/s) and the linear velocity is v = (0·5, 0·5,0)T(m/s). The simulation results of both traditional and CTA case are comparatively depicted in Figures 2–7.
4.2. Scenario 2: ‘Zero- Manoeuvre Motion’
Here the vehicle manoeuvre is taken into consideration, and the consequent estimations of misaligned states are depicted in Figures 8–13.
As depicted in Figures 2–7, in traditional TA framework, if the uniform turn manoeuvre is available, the initial misalignment will converge to less than 0·05 rad and 0·001 (m/s) in nearly 50 seconds, respectively. While in the zero-manoeuvre motion case that as depicted in Figures 8–13, the estimation errors are nearly 0·1 rad and 0·001 (m/s) after 100 s. These results correspond with the fact that the weak observability of instrumental errors will indirectly decrease the evaluating accuracy of attitude and velocity states along with the coupled relationships among them (Rogers, Reference Rogers2002). Meanwhile, provided the proposed CTA framework is adopted, the initial misalignment on attitude and velocity states can approximately convergent to less than 0·01 rad and 0·001 (m/s) respectively in 10 s (the horizon window length of MHE).
5. CONCLUSIONS
In shipborne TA applications, the estimation of unobservable (or weak observable) states is the most time consuming process. In this paper, we consider a novel framework where all instrumental errors and the lever-arm vector are attributed to constraints, which yields rapid convergence of misalignment states. According to the observability analysis results, we can prove its uniform observability even in zero-manoeuvre circumstance. Subsequently, the MHE is adopted to solve the estimation problem and the corresponding stability analysis and simulations are also given to prove the effectiveness of the proposed TA framework.
Future work will focus on more practical cases with more cumbersome constraints and the calculation burden arising during the optimization process. However, along with the increasing power of computers, the framework of MHE-based state estimation will become an alternative for an expanding class of estimation applications, especially in weak observability applications.
ACKNOWLEDGEMENTS
The authors gratefully acknowledge the financial support of the National MOST and GAD through grant 2009CB724000, 2012CB821206 and 613121. The authors also express their thanks to the previous researchers and publishers, whose work inspired our intuition in this work.