1. INTRODUCTION
A ballistic missile is a rocket-propelled flight vehicle with most of its trajectory beyond the atmosphere, that flies in orbit after shutdown and re-enters the Earth by gravity (Zarchan, Reference Zarchan2012). The purpose of a ballistic vehicle is to deliver its warhead towards the intended target with high accuracy, resulting in heavy demands on the Guidance, Navigation and Control (GNC) system. Autonomous navigation with high accuracy and reliability is key for such a demanding application.
Many navigation schemes for ballistic missiles have been researched to realise highly accurate and autonomous navigation (Ali and Fang, Reference Ali and Fang2009; Zhang et al., Reference Zhang, Zheng and Tang2012; Zhang et al., Reference Zhang, Yang, Zhang, Cai and Qian2014; Qian et al., Reference Qian, Sun, Cai and Peng2013; Quan et al., Reference Quan, Li, Gong and Fang2015). The traditional scheme is based on a stand-alone Inertial Navigation System (INS). However, a sole INS is no longer feasible for modern long-range ballistic missiles. This is because even an expensive high quality INS will inevitably suffer from unbounded errors caused by inertial sensor drift. Accumulating over time, such errors may be acceptable initially, but cannot be tolerated for long-range applications. High-level accuracy requirements also bring significant increases in technical difficulty and system cost. Therefore, for accurate navigation of modern ballistic vehicles, the traditional navigation system must be enhanced with other external aids such as a Global Navigation Satellite System (GNSS), Celestial Navigation System (CNS) or Visual Navigation System (VNS). The most widely used external aid is GNSS, which provides accurate position and velocity for civilian and military users, including land vehicles, ships, aircraft and Low Earth Orbit (LEO) satellites. However, the inherent weaknesses, such as plasma sheath effect, vulnerability to interference and accuracy degradation in hostile environments, restrict the use of GNSS for ballistic vehicle navigation (Wen et al., Reference Wen, Zhang and Wu2012; Wang et al., Reference Wang, Xiong, Liu and Shi2016). Another promising aid is CNS, which outputs position and highly accurate attitude information using starlight measurements (Yang et al., Reference Yang, Yang, Zhu and Li2016). Unlike GNSS, CNS is completely autonomous. Its integration with INS can greatly improve navigation performance and is eminently suitable for space missions (Xu and Fang, Reference Xu and Fang2014; Ali and Fang, Reference Ali and Fang2009; Qian et al., Reference Qian, Sun, Cai and Huang2014) and ballistic applications (Qian et al., Reference Qian, Sun, Cai and Peng2013). In addition, a variety of other external aids can be used for Multi-Source Information Fusion (MSIF)-based navigation schemes such as INS/CNS/GNSS (Quan et al., Reference Quan, Li, Gong and Fang2015), INS/VNS/CNS (Ning et al., Reference Ning, Gui, Xu, Bai and Fang2016) and INS/CNS/Geomagnetic (Wang et al., Reference Wang, Zhang and Li2014) methods. Considering autonomy and reliability in practical application, however, the core of MSIF-based methods is still INS/CNS integration (Fang and Ning, Reference Fang and Ning2006). It is worth mentioning that INS/CNS integrated navigation has been successfully applied to Submarine-Launched Ballistic Missiles (SLBM) (Zhang et al., Reference Zhang, Zheng and Tang2012; Zhang et al., Reference Zhang, Yang, Zhang, Cai and Qian2014).
Compared with a platform INS, a Strapdown Inertial Navigation System (SINS) removes the mechanical complexity of the platform by directly mounting inertial sensors on the vehicle, bringing potential benefits of cost-effectiveness, reliability, lightweight, miniature design and low power consumption (Titterton and Weston, Reference Titterton and Weston2004). Stand-alone high-accuracy SINS still cannot be compared with its platform counterparts. However, aiding with CNS, especially accurate star sensors, can greatly enhance the accuracy of SINS (Fang and Ning, Reference Fang and Ning2006). Recently, SINS/CNS integration methods have been extensively researched (Ali and Fang, Reference Ali and Fang2006; Reference Ali and Fang2009; Xu and Fang, Reference Xu and Fang2014; Quan et al., Reference Quan, Li, Gong and Fang2015; Huang et al., Reference Huang, Xie, Shen and Li2016; Wang et al., Reference Wang, Xiong, Liu and Shi2016). The traditional SINS/CNS loose or tight integration adopts starlight information to correct system attitude error and gyro biases, thus restraining accumulated errors caused by misalignment and gyro drifts (Xu and Fang, Reference Xu and Fang2014; Quan et al., Reference Quan, Li, Gong and Fang2015; Wang et al., Reference Wang, Xiong, Liu and Shi2016). Based on this framework, accurate attitude estimation can be realised (Huang et al., Reference Huang, Xie, Shen and Li2016), but the accelerometer biases are unobservable which may cause a divergence of other navigation errors. Advanced nonlinear filtering methods such as Unscented Kalman Filter (UKF) and Unscented Particle Filter (UPF) are also introduced to address the model nonlinearities and disturbances, and these methods show improvements in navigation accuracy with still unobservable accelerometer biases (Ali and Fang, Reference Ali and Fang2006; Reference Ali and Fang2009). It should be noted that this problem can be solved for vehicles on the surface of planets by maintaining the navigation frame (mathematical horizontal reference), which is needed to provide latitude and longitude for rovers or explorers (Ning and Liu, Reference Ning and Liu2014; He et al., Reference He, Wang and Fang2014; Guan et al., Reference Guan, Wang, Fang and Feng2014; Ning et al., Reference Ning, Gui, Xu, Bai and Fang2016). For vehicles flying beyond the atmosphere, the accelerometer biases cannot be estimated accurately without the existence of a horizontal reference. The stellar refraction-based SINS/CNS integration is realised by using the stellar refraction method to indirectly sense the Earth's horizon reference (Qian et al., Reference Qian, Sun, Cai and Peng2013; Ning et al., Reference Ning, Wang, Bai and Fang2013; Wang et al., Reference Wang, Zhang and Li2014; Qian et al., Reference Qian, Sun, Cai and Huang2014; Yang et al., Reference Yang, Yang, Zhu and Li2016). The simulations show a high positioning accuracy of less than 100 m can be reached with a proper atmospheric refraction model. However, this method will undergo significant accuracy degradation due to the inevitable atmospheric density error. Moreover, the accelerometer biases can only be estimated accurately when the number of refraction stars reaches three (Qian et al., Reference Qian, Sun, Cai and Peng2013). But the fact is that observations with more than three refraction stars are hard to obtain even with a large Field-Of-View (FOV) star sensor. Fewer refraction stars might cause a divergence of navigation errors. Therefore, the stellar refraction-based method still needs improvements.
In an analogy with Micro-Electromechanical Systems (MEMS)-based land navigation, the aiding of Nonholonomic Constraints (NHC) or Zero Velocity Updates (ZUPTs) can enhance SINS performance without the addition of other external sensors (Dissanayake et al., Reference Dissanayake, Sukkarieh, Nebot and Durrant-Whyte2001; Niu et al., Reference Niu, Li, Zhang, Cheng and Shi2012). These constraints have been adopted for autonomous navigation of lunar rovers (Ning and Liu, Reference Ning and Liu2014) and Mars explorers (He et al., Reference He, Wang and Fang2014; Guan et al., Reference Guan, Wang, Fang and Feng2014), indicating significant improvements in accuracy. This inspires us to find model constraints for a ballistic vehicle. To address the limitations encountered by traditional SINS/CNS methods, aiding from model constraints should be introduced to correct SINS errors. Fortunately, as far as we are aware, a weightlessness constraint always exists when a ballistic vehicle flies beyond the atmosphere. The complete weightlessness means the accelerometer output should be zero. This useful model constraint seems promising in overcoming the limitations.
Motivated by this idea and the work of Dissanayake et al. (Reference Dissanayake, Sukkarieh, Nebot and Durrant-Whyte2001) and Qian et al. (Reference Qian, Sun, Cai and Peng2013), this paper proposes a new constrained SINS-based integrated navigation approach for ballistic vehicles to improve accuracy. Our algorithm exploits the complete weightlessness constraints in the free flight phase to obtain an accelerometer bias observation, which handles the unobservable accelerometer biases problem. The information filter is devised to fuse the multi-rate observations from multiple sources, i.e. SINS, CNS and model constraints. In the filter, improvements to the dynamic equations are used to reduce the propagation of navigation errors, and high-rate virtual constraints are used to reduce the impact of bias errors in the accelerometer data. The proposed method is also evaluated by long range ballistic vehicle simulations.
The structure of this paper is as follows. Section 2 presents the derivations of model constraints. Section 3 devises an information filter for multi-rate observations. The simulation results and conclusions are given in Section 4 and Section 5. The contribution of this paper is to provide an effective way to improve the traditional SINS-based integrated navigation performance without adding new external sensors.
2. MODEL CONSTRAINTS FOR A BALLISTIC VEHICLE
2.1. Model constraints for inertial navigation in orbit
The motion of a ballistic vehicle can be described clearly in the inertial frame (J2000.0), in which the navigation equations are formulated (Titterton and Weston, Reference Titterton and Weston2004):
$$\left\{ {\matrix{ \dot{\bi{C}}_{i}^{\,b} = -[ {{\bi{\omega}}}_{ib}^{b} \times \rsqb {\bi{C}}_{i}^{b} \hfill\cr \dot{{\bi{v}}}^{i} = {\bi{C}}_{b}^{i} {\bi{f}}^{b} + {\bi{g}}^{i} \hfill \cr \dot{{\bi{r}}}^{i} = {\bi{v}}^{i}\hfill \cr}}\right.$$
where
${{\bi{C}}}_{i}^{b}$
represents the transformation matrix from the inertial frame (i-frame) to SINS body frame (b-frame), which corresponds to Euler angles
$[ \phi\comma \; \theta\comma \; \psi\rsqb ^{T}$
(i.e. roll, pitch and yaw with 3-2-1 rotation sequence).
$\bi{{v}}^{i}$
is the velocity vector referenced in the i-frame,
$\bi{{r}}^{i}$
is a position vector resolved in the i-frame,
${\bi{\omega}}_{ib}^{b}$
is the angular rate between the i-frame and b-frame resolved in the b-frame,
$\bi{{f}}^{b}$
is the specific force resolved in the b-frame, the operator
$[ \lpar{\cdot}\rpar \times\rsqb$
represents a skew-symmetric matrix of a vector and
$\bi{{g}}^{i}$
is the gravity vector resolved in the i-frame, which can be computed by Equation (2), considering the second zonal harmonic terms of the Earth Ellipsoid model
$${\bi{g}}^{i} = -\left\{{\bi{I}} + J_2 \left(\displaystyle{{R_e}\over{r}}\right)^2 \left[{\bi{c}} - 7{\cdot}5 \left(\displaystyle{{r_z}\over{r}}\right)^2 {\bi{I}} \right] \right\} \displaystyle{{\mu}\over{r^3}} {\bi{r}}^{i}$$
In Equation (2), J
2 is the Earth's second zonal harmonic constant
$1{\cdot}082627 \times 10^{-3}$
, R
e
is the Earth's radius, μ is the Earth's gravitational constant
$3{\cdot}986 \times 10^{14}\, \hbox{m}^{3}\, \hbox{s}^{-2}$
,
$\bi{{I}}$
is the identity matrix,
$\bi{{c}}$
is the constant diagonal matrix with diagonal elements
${diag}({\bi{{c}}})=[1{\cdot}5,1{\cdot}5,4{\cdot}5]^{T}$
, r
z
is the z-axis component of position vector
${\bi{r}}^{i} = [ r_{x}\comma \; r_{y}\comma \; r_{z}\rsqb ^{T}$
and
$r=\vert \bi{{r}}^{i} \vert$
.
The flight of a ballistic missile can be divided into two parts: powered flight and free flight. Powered flight is mainly distributed inside the atmosphere and has a short duration. This includes necessary attitude adjustments for star gazing or manoeuvring outside the atmosphere. For most of the flight time, the vehicle flies beyond the atmosphere with all engines off. Because only gravity is applied in this phase, it is free flight. Theoretically speaking, accelerometer output should be zero in the complete weightlessness condition. This means that the non-zero output can be regarded as bias and noise. To reduce the propagation of navigation errors, the accelerometer output should be zero during free flight. Thus, Equation (1) is rewritten as
$$\left\{ {\matrix{ \dot{{\bi{C}}}_{i}^{b} = -[ {\bi{\omega}}_{ib}^{b} \times \rsqb {\bi{C}}_{i}^{b}\hfill \cr \dot{{\bi{v}}}^{i} = {\bi{cC}}_{b}^{i} {\bi{f}}^{b} + {\bi{g}}^{i} \hfill \cr \dot{{\bi{r}}}^{i} = {\bi{v}}^{i}\hfill \cr}}\right.$$
where c = 1 means the vehicle is in powered flight; c = 0 means it is in free flight. Easy judgements can be made from the engine commands.
Once the initial position, velocity and attitude are determined, the navigation solutions can be computed with inertial data (i.e.,
${\bi{\omega}}_{ib}^{b}$
and
${\bi{f}}^{b}$
) using Equations (2) and (3). The i-frame mechanisation is shown in Figure 1. More details on the SINS navigation algorithm can be found in Titterton and Weston (Reference Titterton and Weston2004).
Figure 1. SINS navigation - Inertial frame mechanisation.
2.2. Virtual observations due to model constraints
The inertial sensor errors can be divided into two categories, i.e. systematic errors and stochastic errors. The former can be eliminated from raw measurements by calibration; while the latter are the residuals after calibration, which can be modelled by stochastic processes to be included in the SINS error model. In this paper, the inertial residuals are modelled as (Qian et al., Reference Qian, Sun, Cai and Peng2013; Ning et al., Reference Ning, Wang, Bai and Fang2013):
$$\left\{ {\matrix{ {{\hat{\bi f}}^b = {\bi f}^b + {\bi b}_f + \delta {\bi f}^b}\hfill \cr {\hat{{\bi {\omega} }}_{ib}^b = {\bi {\omega}}_{ib}^b + {\bi b}_\omega + \delta {\bi \omega }_{ib}^b } \hfill\cr {{\mathop {\bi b}\limits^. }_f = {\bi 0}}\hfill \cr {{\mathop {\bi b}\limits^.}_\omega = {\bi 0}}\hfill \cr }}\right.$$
where
$\hat{{\bi{f}}}^{b}$
and
$\hat{\bi{\omega}}_{ib}^{b}$
are the actual SINS output after calibration;
${\bi{f}}^{b}$
and
${\bi{\omega}}_{ib}^{b}$
are the true values of inertial measurements; accelerometer residuals are modelled as the hybrid of bias
${\bi{b}}_{f}$
and noise
$\delta {\bi{f}}^{b}$
, and gyro residuals as the hybrid of bias
${\bi{b}}_{\omega}$
and noise
$\delta {\bi{\omega}}_{ib}^{b}$
;
${\bi{b}}_{f}$
,
${\bi{b}}_{\omega}$
are random constants and
$\delta {\bi{f}}^{b}\comma \; \delta {\bi{\omega}}_{ib}^{b}$
are white noise,
$\delta {\bi{f}}^{b} \sim N \lpar 0\comma \; \sigma_{b}^{2}\rpar \comma \; \delta {\bi{\omega}}_{ib}^{b} \sim N \lpar 0\comma \; \sigma_{\omega}^{2}\rpar $
.
In fact, the vehicle is completely weightless in free flight, which occupies most of the flight time. The accelerometer output is expected to be zero, i.e.
${\bi{f}}^{b} = {\bi{0}}$
. The non-zero output corresponds to the accelerometer bias, as shown in Equation (5).
$$\hat{{\bi{f}}}^{b} = {\bi{b}}_{f} + \delta {\bi{f}}^{b}$$
The fact that the theoretical output of the accelerometer in free flight is zero is referred to as a “virtual observation”. Its proper use is expected to enhance the standalone SINS performance.
2.3. Estimation of the vehicle state in the presence of constraints
The discrete error state equation for SINS/CNS integrated navigation system can be expressed:
$${\bi{x}}_{k/k-1} = {\bi{\varPhi}}_{k,k-1}\, {\bi{x}}_{k-1} + {\bi{\varGamma}}_{k-1} {\bi{w}}_{k-1}$$
where the system error state is defined as
$x = \left[ {\matrix{ {\delta {\bf r}}^i & {\delta {\bf v}}^i & {\bi{\theta}} & {{\bf b}_\omega} & {{\bf b}_f}} } \right]^T ; {\bi{x}}_{k-1}$
represents the state estimate at time (k − 1);
${\bi{x}}_{k/k-1}$
denotes the predicted state estimate at time k; the dynamic noise is
${\bi w}_k = \left[ {\matrix{ {\Delta {\bi f}_k^b } & {\Delta {\bi{\omega}}_{ib{\rm ,}k}^b }} } \right]^T$
;
${\bi {\varPhi}}_{k\comma k-1}$
is the transition matrix from state
${\bi{x}}_{k-1}$
to state
${\bi{x}}_{k/k-1}$
. Equation (6) is obtained by the perturbation analysis of the SINS mechanisation Equations (2) and (3) (Titterton and Weston, Reference Titterton and Weston2004; He et al., Reference He, Li and Zhang2013), and detailed derivations are listed in Appendix A. According to the covariance propagation formula, the prediction-error covariance matrix
${\bi{P}}_{k/k-1}$
is computed
$${\bi{P}}_{k/k-1} = {\bi{\varPhi}}_{k,k-1} {\bi{P}}_{k-1/k-1} {\bi{\varPhi}}_{k,k-1}^T +{\bi{\varGamma}}_{k-1} {\bi{Q}}_{k-1} {\bi{\varGamma}}_{k-1}^T$$
where
${\bi{Q}}_{k-1}$
is the covariance matrix of the inertial measurement noise.
In Equation (6), position error
$\delta {\bi{r}}^{i}$
, velocity error
$\delta {\bi{v}}^{i}$
, misalignment
${\bi {\theta}}$
, accelerometer measurement error
$\Delta {\bi{f}}^{b}$
and gyro measurement error
$\Delta {\bi {\omega}}_{ib}^{b}$
are defined as follows:
$$\left\{ {\matrix{ {\delta {\bi r}^i = {\hat{\bi r}}^i - {\bi r}^i} \hfill\cr {\delta {\bi v}^i = {\hat{\bi v}}^i - {\bi v}^i} \hfill\cr {[{\bi{\theta}} \times ] = {\bi I} - \hat{\bi C}_i^b {\bi C}_b^i } \hfill\cr {\delta {\bi g}^i = {\hat{\bi g}}^i - g^i} \hfill\cr {\Delta {\bi f}^b = {\hat{\bi f}}^b - {\bi f}^b} \hfill\cr {\Delta {\bi {\omega}}_{ib}^b = \hat{{\bi {\omega}}}_{ib}^b - {\bi{\omega}}_{ib}^b }\hfill\cr} } \right.$$
“^” denotes the variable with error, “Δ” or “δ” denotes the difference between the actual value and the true value;
$\Delta {\bi{f}}^{b}$
and
$\Delta {\bi {\omega}}_{ib}^{b}$
can be obtained from Equation (4), and the discrete constraint in the free flight phase is obtained from Equation (5) as follows:
$${\bi{z}}_k^{virtual} = \hat{{\bi{f}}}_{k}^{b} - {\bi{0}} = {\bi{b}}_{f{\!\!},k} + \delta {{\bi{f}}}_{k}^{b}$$
in which the non-zero virtual measurement
${\bi{z}}_{k}^{virtual}$
is expected to be zero.
Subjected to the stochastic constraint in Equation (9), the estimation for system state
${\bi{x}}_{k}$
described by Equation (6) can be implemented based on an Extended Kalman Filtering (EKF) framework. The virtual observation can be the sole aiding or combined with other external aiding such as CNS. However, the update rate among these measurements may be totally different. For easy implementation, an Information Filter (IF) is devised to fuse the multi-rate data, shown in Figure 2.
Figure 2. The algorithm structure for a SINS/CNS integrated system aided by virtual observations.
3. THE INFORMATION FILTERING APPROACH FOR MULTI-RATE OBSERVATIONS
Mathematically, the information filter is equivalent to a Kalman filter, thus obtaining the same results. The major difference is that the IF is developed in the information space instead of the state space. This brings the following benefits: state estimation without the initial value (a priori information), and easy implementation without concerns about observation correlation from different sources. These aspects lead to the wide use of IF in multi-source information fusion applications. The reader can refer to Bar-Shalom et al. (Reference Bar-Shalom, Li and Kirubarajan2001) for more detailed derivations of IF and its applications in multi-source information fusion.
In this paper, the multi-source information mainly includes a virtual constraint (discussed in Section 2.2), attitude observation (i.e. direct starlight information), and apparent height observation (i.e. refraction information). Due to its easy implementation and potential extension to newer external observations, a real-time and efficient information filter is devised.
3.1. The Information Filter approach
Two key concepts for IF are information matrix Y and filter state y. According to the duality between the covariance and information, the information matrix is the inverse of the covariance matrix
$${\bi{Y}}_k = {\bi{P}}_k^{-1}$$
Thus, the information filter state is defined as
$${\bi{y}}_k = {\bi{P}}_k^{-1} {\bi{x}}_k$$
where
${\bi{x}}_{k}$
is the system state vector at time k. A one-step prediction of IF state
${\bi{y}}_{k/k-1}$
is obtained
$${\bi{y}}_{k/k-1} = {\bi{Y}}_{k/k-1} {\bi{\varPhi}}_{k,k-1} {\bi{Y}}_{k-1}^{-1} {\bi{y}}_{k-1}$$
The corresponding information matrix is
$${\bi{Y}}_{k/k-1} = \left[{\bi{\varPhi}}_{k,k-1} {\bi{Y}}_{k-1}^{-1} {\bi{\varPhi}}_{k,k-1}^{T} +{\bi{\varGamma}}_{k-1} {\bi{Q}}_{k-1} {\bi{\varGamma}}_{k-1}^{T} \right]^{-1}$$
where
${\bi {\varGamma}}_{k-1} {\bi{Q}}_{k-1} {\bi {\varGamma}}_{k-1}^{T}$
is the process noise of the system model
${\bi {\varPhi}}_{k\comma k-1}$
.
Once the measurement
${\bi{z}}_{k}$
is available, the IF measurement vector
${\bi{i}}_{k}$
and its corresponding covariance matrix
${\bi{I}}_{k}$
are
$${\bi{i}}_k = {\bi{H}}_{k}^{T} {\bi{R}}_{k}^{-1} {\bi{z}}_k$$
$${\bi{I}}_k = {\bi{H}}_{k}^{T} {\bi{R}}_{k}^{-1} {\bi{H}}_k$$
where
${\bi{H}}_{k}$
represents the measurement model and
${\bi{R}}_{k}$
is the measurement noise matrix. In fact,
${\bi{i}}_{k}$
can be understood as the contribution of measurement
${\bi{z}}_{k}$
to IF state update, and
${\bi{I}}_{k}$
is the measure for information in
${\bi{z}}_{k}$
projected on the IF state. The estimate with measurement update is
$${\bi{y}}_k = {\bi{y}}_{k/k-1} + {\bi{i}}_{k}$$
$${\bi{Y}}_k = {\bi{Y}}_{k/k-1} + {\bi{I}}_{k}$$
The advantage brought by using Equations (16) and (17) lies in the multi-source observations case. It is obvious that the information filter state and information matrix are merely the sum of individual information filter states and the information matrix, i.e.
$${\bi{y}}_k = {\bi{y}}_{k/k-1} + \sum\limits_{j=1}^{N} {\bi{i}}_{k}^{j}$$
$${\bi{Y}}_k = {\bi{Y}}_{k/k-1} + \sum\limits_{j=1}^{N} {\bi{I}}_{k}^{j}$$
in which N represents the number of aiding sources.
3.2. Multi-rate observation equations
When an aiding sensor provides its observation information, the measurement z k can be constructed as the difference between this observation and the corresponding SINS navigation solution. In this paper, possible measurements include virtual constraints, attitude observation and stellar refraction observation.
3.2.1. Virtual constraints
As discussed in Section 2.2, the accelerometer output is subjected to stochastic constraint when a ballistic vehicle is in free flight. This stochastic constraint can be regarded as a virtual constraint (Virtual Accl ) in the SINS b-frame.
$$\left\{ {\matrix{{Virtual_{Acclx} = 0} \cr {Virtual_{Accly} = 0} \cr {Virtual_{Acclz} = 0}} } \right.$$
The accelerometer measurement noise is denoted as
${\bi{R}}_{k}^{Accl}$
. In free flight, the actual accelerometer output referenced in the b-frame can be written as:
$${\bi{z}}_k^{virtual} = Inertial_k^{Accl} - Virtual_k^{Accl}$$
The measurement design matrix is
$${\bi H}_k^{virtual} = \left[ {\matrix{ {{\bi 0}_{3 \times 3}} & {{\bi 0}_{3 \times 3}} & {{\bi 0}_{3 \times 3}} & {{\bi I}_{3 \times 3}} & {{\bi 0}_{3 \times 3}}}} \right]$$
The measurement noise covariance matrix is
$${\bi R}_k^{virtual} = \left[ {\matrix{ {R_k^{Acclx} } & 0 & 0 \cr 0 & {R_k^{Accly} } & 0 \cr 0 & 0 & {R_k^{Acclz}}}} \right]$$
where x, y and z represent the sensitive axis; the diagonal element is given by bias instability.
3.2.2. Attitude observation
Using the direct stars sensed by star sensor, attitude matrix
$\hat{{\bi{C}}}_{i}^{s}$
from the i-frame to the star-sensor frame (s-frame) can be obtained. Thus, the star sensor observation vector
${\bi{z}}_{k}^{\theta}$
is the misalignment between the i-frame and the SINS b-frame, which satisfies
$$[{\bi{z}}_k^{\theta} \times ] = {\bi{I}} - \hat{{\bi{C}}}_{i}^{s} \tilde{{\bi{C}}}_{b}^{i} \overline{{\bi{C}}}_{s}^{b}$$
where
$\tilde{{\bi{C}}}_{b}^{i}$
is the attitude matrix computed by SINS.
$\overline{{\bi{C}}}_{b}^{s}$
is the designed installation matrix from SINS b-frame to star sensor s-frame, which is assumed to be measured accurately in advance.
The measurement design matrix is
$${\bi H}_k^\theta = \left[ {\matrix{ {{\bi 0}_{3 \times 3}} & {{\bi 0}_{3 \times 3}} & { - {\bi C}_b^s } & {{\bi 0}_{3 \times 3}} & {{\bi 0}_{3 \times 3}}} } \right]$$
The corresponding measurement noise covariance matrix is
$${\bi R}_k^\theta = \left[ {\matrix{ {\varepsilon _k^x } & 0 & 0 \cr 0 & {\varepsilon _k^y } & 0 \cr 0 & 0 & {\varepsilon _k^z }}} \right]$$
3.2.3. Stellar refraction observation
The refraction star detection should be conducted after star identification. If refraction stars are detected, the star sensor provides the refraction angle a
r
and the refraction star direction vector
${\bi{e}}^{\prime}$
. In Figure 3, the apparent height h
a
can be computed according to geometry.
$$h_a = \sqrt{{\bi{r}}_{s}^{T} {\bi{r}}_{s} -({\bi{e}}^{\prime T} {\bi{r}}_s)^{T} {\bi{e}}^{\prime T} {\bi{r}}_s} - R_e$$
Figure 3. Starlight refraction.
Once the refraction angle a
r
is measured by star sensor, the apparent height
$$\tilde h_a$$
and tangent height
$$\tilde h_g$$
can be obtained using the atmospheric refraction model (White and Gounley, Reference White and Gounley1987) listed below.
$$\eqalign{{\tilde h}_a &= {\tilde h}_g + a_r\cdot \sqrt {H\cdot R_e/2\pi } \cr & = h_a + \upsilon _a}$$
where H is the density scale height; h a is the theoretical true value and υ a is the stochastic error caused by refraction angle measurement noise. Usually, refraction star observation is restricted in the tangent height [20 km, 50 km] (H = 6·366 km is suitable in this range), which satisfies the following empirical formula (Wang et al., Reference Wang, Ning, Jin and Sun2004)
$$\tilde{h}_g = 58{\cdot}29096 - 6{\cdot}587501 \cdot \ln (a_r)$$
The apparent height observation is
$$\eqalign{ {\bi{z}}_k^{h_a} &= \hat{h}_a - \tilde{h}_a \cr &=\left(\sqrt{(\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)^T (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s) - (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)^T {\bi{e}}^{\prime} {\bi{e}}^{\prime T} (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)} - R_e \right) - \tilde{h}_a }$$
where
$\hat{{\bi{r}}}^{i}$
and
$\hat{{\bi{C}}}_{b}^{i}$
are the position and attitude matrices calculated by SINS and
$\overline{{\bi{l}}}_{s}$
is the lever-arm vector from the star-sensor to the SINS centre in the b-frame, which can be measured accurately in advance.
The corresponding measurement design matrix is
$${\bi H}_k^\theta = \left[ {\matrix{ {{\bi M}_{\bi r}} & {{\bi 0}_{3 \times 3}} & { - {\bi M}_r\hat{{\bi C}}_b^i [{\overline {\bi l} }_s \times ]} & {{\bi 0}_{3 \times 3}} & {{\bi 0}_{3 \times 3}}}} \right]$$
in which
$ {\bi{M}}_{{\bi{r}}} = \displaystyle{{[ \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar - {\bi{e}}^{\prime} {\bi{e}}^{\prime T}\ \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar \rsqb ^{T}}\over{\sqrt{\lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar ^{T} \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar - \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar ^{T}\ {\bi{e}}^{\prime} {\bi{e}}^{\prime T}\ \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar }}}$
.
According to the covariance propagation formula, the variance of apparent height observation is calculated by perturbing Equation (28),
$$R_k^z = \left( {\sqrt {\displaystyle{{HR_e} \over {2\pi }}} - \displaystyle{{6587\cdot 501} \over {a_r}}} \right)^2\sigma _{a_r}^2 $$
in which
$\sigma_{a_{r}}^{2}$
is the measurement noise covariance of refraction angle a
r
provided by the star sensor.
4. SIMULATION RESULTS
This section presents the results of three simulations with a representative long-range ballistic vehicle trajectory. The simulation scenarios include three cases: (1) stand-alone SINS navigation, (2) attitude-based SINS/CNS integrated navigation, and (3) stellar refraction-based SINS/CNS (or SINS/RCNS) integrated navigation. The simulations are primarily aimed at demonstrating the use of model constraints (presented in Section 2) and an information filter (Section 3) for improving the ballistic navigation performance.
A representative long-range (10,000+ km) ballistic trajectory is generated as shown in Figure 4. Launch time is 00:00:00, 1 January 2015 (UTC), and the whole flight time is 2,700 sec. The initial position is (40°N, 116°E). The vehicle leaves the atmosphere at t = 100 s and shuts down at t = 180 s. The star sensor works beyond the atmosphere, while star observations are made only in six segments, i.e. [400 s, 410 s], [1,000 s, 1,010 s], [1,550 s, 1,560 s], [2,100 s, 2,110 s], [2,550 s, 2,560 s] and [2,650 s, 2,660 s]. The rest of the flight is used to make preparations such as attitude adjustments.
Figure 4. The representative long-range ballistic trajectory.
The simulation parameters for the navigation devices are listed in Table 1. For the missile-borne SINS simulator, the gyro bias repeatability and stability are 0·01°/h, and the accelerometer bias repeatability and stability are 50 μg. For the star sensor simulator, the FOV is assumed to be 12° × 12° with 1024 × 1024 pixels. The image point extraction accuracy is 0·01 pixels. Figure 5 shows the number of refraction stars observed by the simulated star sensor during the whole flight time.
Figure 5. The number of refraction stars during the whole flight time.
Table 1. Simulation parameters for navigation devices in the representative trajectory.
Simulation parameters are set according to Table 1, and the accuracy comparisons between the proposed algorithm and traditional SINS/CNS approaches are provided over 100 Monte Carlo trials in Figures 6 to 11 and Table 2. In Figures 6 to 11, the blue line represents the three-dimensional navigation error (position/velocity/attitude) in one trial, while the red line represents the theoretical covariance bounds (3σ) for the corresponding three-dimensional navigation error.
Figure 6. Monte Carlo trials estimted errors (blue) and 3σ outlier (red) of stand-alone inertial navigation without constraints.
Figure 7. Monte Carlo trials estimted errors (blue) and 3σ outlier (red) of constrained stand-alone inertial navigation.
Figure 8. Monte Carlo trials estimted errors (blue) and 3σ outlier (red) of traditional SINS/CNS integrated navigation.
Figure 9. Monte Carlo trials estimted errors (blue) and 3σ outlier (red) of constrained SINS/CNS integrated navigation.
Figure 10. Monte Carlo trials estimted errors (blue) and 3σ outlier (red) of traditional SINS/RCNS integrated navigation.
Figure 11. Monte Carlo trials estimted errors (blue) and 3σ outlier (red) of constrained SINS/RCNS integrated navigation.
Table 2. The summary of accuracies in three cases for a long-range ballistic navigation.
4.1. Case 1: Stand-alone SINS
Case 1 is used to evaluate the effectiveness of the algorithm proposed in Section 2 with SINS as the only navigation device. Figures 6(a) and 7(a) show the errors in position, velocity and attitude obtained by free inertial navigation and constrained inertial navigation (i.e., aided by virtual observations) through the Monte Carlo trials. It is anticipated that almost all trial errors (blue line) should be below the theoretical standard variance (3σ, red line), which is illustrated in Figures 6(a) and 7(a). For the representative trajectory, the inertial navigation position Root Mean Square Error (RMSE) at t = 2700 s (end time) decreases from 3148·7304 m to 1712·0156 m, which is a 45·63% improvement in accuracy brought by virtual constraints. The velocity RMSE undergoes a similar drop from 3·7982 m/s to 1·1354 m/s, that is, a 70·11% improvement in accuracy brought by virtual constraints.
The comparison between Figures 6(b) and 7(b) shows that the accelerometer biases are estimated more accurately due to better observability caused by the addition of the virtual constraint. Here, the attitude error is defined as the square-root of the quadratic sum of three misalignment angles, which describes the overall attitude accuracy around an axis. However, the attitude RMSE is unchanged regardless of whether the virtual aiding is used, as shown in Figures 6(a) and 7(a). This is because the gyro bias estimation is barely influenced by the virtual constraint, which is only an explicit measurement of accelerometer biases. Therefore, the stand-alone SINS results demonstrate the use of the virtual constraint for improving the SINS position and velocity accuracy.
It is obvious that the use of the virtual constraint enhances the inertial navigation without any extra sensors, thus reducing dependence on external information for stand-alone SINS.
4.2. Case 2: Attitude-based SINS/CNS integrated navigation
A traditional SINS/ CNS integrated system only uses information from direct starlight (i.e. attitude) to fuse with the inertial data. Case 2 is used to evaluate the effectiveness of model constraints in Section 2 for traditional SINS/CNS methods.
Using information from direct starlight, traditional SINS/CNS can estimate gyro biases to obtain accurate attitude, which could also limit the growth of position and velocity errors to some extent. Figures 8(a) and 9(a) show the errors in position, velocity and attitude obtained by SINS/CNS and constrained SINS/CNS navigation (i.e., aided by virtual observation) through the Monte Carlo trials. Almost all the trial errors (blue) are below the theoretical standard variance (3σ, red line) in Figures 8(a) and 9(a), but a few trials exceed the theoretical 3σ outlier line in Figure 8(a). This difference indicates that model constraints are beneficial to the filter's convergence and stability. For the same ballistic trajectory, the introduction of CNS contributes to a significant improvement in attitude accuracy by about 98·84% (from 39·5620″ to 0·4596″). The better attitude estimation also brings minimal improvements in position and velocity errors of about 14·35% (from 3148·7304 m to 2697·0040 m) and 3·10% (from 3·7982 m/s to 3·6806 m/s). On this basis, traditional SINS/CNS can be enhanced with the addition of virtual aiding as shown in Figure 9(a). Compared with traditional SINS/CNS, the constrained SINS/CNS shows a sharp increase in position and velocity accuracy of about 79·90% (from 2697·0040 m to 542·1024 m) and 82·10% (from 3·6806 m to 0·6598 m) with the same high attitude accuracy. This demonstrates that the virtual constraint can improve position and velocity accuracy without influence upon gyro biases and attitude estimation.
Similarly, an accuracy comparison is conducted between constrained SINS and constrained SINS/CNS. Obvious improvements can be observed in position, velocity and attitude accuracy. There are significant increases in navigation accuracy of about 68·34% (from 1712·0156 m to 542·1024 m), 41·91% (from 1·1354 m/s to 0·6598 m/s) and 98·84% (from 39·5620″ to 0·4596″). It is clear that the larger improvements are brought by virtual aiding (i.e., accelerometer biases estimation), not by the introduction of CNS. This indirectly indicates that the accelerometer plays a major role in affecting system accuracy for long-range ballistic missiles.
The estimation of inertial sensor biases is illustrated in Figures 8(b) and 9(b). Figure 8(b) shows that the adoption of a star sensor can estimate gyro bias accurately, but it fails to estimate the accelerometer bias. However, along with virtual observations, the accurate estimation of both gyro and accelerometer biases can be realised, as shown in Figure 9(b).
4.3. Case 3: Stellar-refraction-based SINS/CNS integrated navigation
Based on the information filter framework, a new constrained SINS/RCNS integrated navigation scheme is presented in Sections 2 and 3. This approach adopts all possible information available, including virtual constraints, direct starlight-induced attitudes and stellar refraction angles to aid SINS.
The refraction angle and apparent height information can improve the observability of the filter states, thus further enhancing the accuracy of the traditional SINS/CNS scheme (Qian et al., Reference Qian, Sun, Cai and Peng2013; Ning et al., Reference Ning, Wang, Bai and Fang2013; Wang et al., Reference Wang, Zhang and Li2014; Qian et al., Reference Qian, Sun, Cai and Huang2014; Yang et al., Reference Yang, Yang, Zhu and Li2016). The SINS/RCNS integration approach can reach a high positioning accuracy of 81·05 m (1σ), velocity accuracy of 0·16 m/s (1σ) and attitude accuracy of 0·47″ (1σ). Compared with the traditional SINS/CNS scheme, this is a great boost to navigation accuracy, since both position and velocity accuracy increase by more than 95% with the same high attitude accuracy.
It can be observed in Figure 10(a) that the blue navigation error lines exceed the theoretical 3σ outlier red line occurring in many trials, which is unnatural for Kalman filtering. Considering this phenomenon rarely happens in SINS/CNS cases, it is reasonable to doubt that it may be caused by the introduction of refraction information. Further investigation into the number of refraction stars (shown in Figure 5) reveals that the observations with no more than two refraction stars occupy more than 70% of star-sensor working time. According to the findings in Qian et al. (Reference Qian, Sun, Cai and Peng2013), the KF can obtain sufficient information to estimate the accelerometer biases when the number of refraction stars reaches three. This means some states are unobservable for most of the flight time under the simulation conditions in this paper, which may result in inaccurate filter estimation or even divergence. The accelerometer bias estimation is shown in Figure 10(b), which is inaccurate, as expected. This demonstrates that the unnatural phenomenon is due to unobservable accelerometer bias. In this situation, the virtual constraint is introduced to realise constrained SINS/RCNS integrated navigation. Fortunately, the introduction of the virtual constraint tackles the filtering divergence problem manifested in Figure 10. It can be observed in Figure 11(a) that all the trial errors never exceed the theoretical 3σ outlier red line. Both accelerometer bias and gyro bias are estimated accurately in Figure 11(b). The proposed constrained SINS/RCNS scheme addresses the navigation performance degradation problem when the number of refraction stars is small (less than three).
In addition, compared with the traditional SINS/RCNS scheme, the proposed constrained algorithm maintains the high level of attitude accuracy, and further improves the positioning and velocity accuracy from 81·05 m and 0·16 m/s to 58·98 m and 0·05 m/s. It is verified that the virtual constraint can effectively improve the SINS/RCNS accuracy without adding extra sensors.
All three case results show that the constrained algorithm enhances the performance of SINS-based integrated navigation as a whole.
4.4. Summary of Results
Table 2 provides a summary of navigation accuracies between the traditional algorithm and the proposed method in all three cases. It can be seen that the actual accuracy obtained by Monte Carlo trials is consistent with the theoretical accuracy.
5. CONCLUSIONS
The demands for highly accurate, reliable and autonomous navigation mean that SINS is often used in long-range ballistic vehicle applications. However, SINS suffer from rapid error growth with time due to the inertial sensor drifts caused by nonlinearities and noise. This paper proposes a virtual constraint to control SINS error growth without adding an extra sensor. The proposed constrained algorithm effectively enhances inertial navigation and makes missile-borne SINS less dependent on external information. It can be anticipated that the accurate performance time of SINS can be lengthened using the proposed algorithm with the same accuracy requirements as before.
The key to the proposed algorithm is the full use of the constraints that govern the motion of a ballistic vehicle in the free flight phase. The accelerometer output is theoretically zero in the complete weightlessness state, thus creating a virtual observation for accelerometer bias. Moreover, a CNS uses direct starlight to determine accurate attitude information for the vehicle, which is a high-quality observation for attitude and gyro bias. As a result, the combination of a virtual constraint and CNS can tackle the navigation error divergence problem caused by unobservable accelerometer bias, and improve the accuracy of traditional SINS/CNS integrated navigation.
The information filter framework is convenient for multi-rate data fusion from multiple sources. The implementation presented here can be easily extended to deal with new information provided by a sun sensor, geomagnetic sensor, GNSS, ground station and any other meaningful constraints. It is worth mentioning that the constrained algorithm can further bound the positioning and velocity errors of the SINS/RCNS integrated system. Moreover, this technique can address the filter divergence problem encountered by a traditional SINS/RCNS integrated navigation systems with few refraction stars, thus enhancing the navigation performance.
Based on the simulation results, it is recommended that the use of the virtual constraint be considered for the accurate navigation of vehicles flying beyond the atmosphere.
APPENDIX A. THE DERIVATION OF CONSTRAINED DISCRETE SINS ERROR STATE EQUATIONS
Perturbation analysis is conducted to analyse the error propagation of SINS/CNS integrated navigation system. According to Equations (2) and (8), the perturbed gravity is approximated by truncating the Taylor series to its first-order:
$$\delta {\bi{g}}^i = {\bi{G}} ({\bi{r}}^i) \delta {\bi{r}}^i$$
where
${\bi{G}} \lpar {\bi{r}}^{i}\rpar $
is a matrix function of position vector
${\bi{r}}^{i}$
, and its element G
ij
is defined in Equation (A2).
$$ \left\{ {\matrix{ G_{11} = \displaystyle{{\mu (3x^2 - r^2)}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 1{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu x}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 x}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-1 \right) \right] \hfill\cr G_{12} = \displaystyle{{3\mu xy}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 1{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu x}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 y}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-1 \right) \right] \hfill\cr G_{13} = \displaystyle{{3\mu xz}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 1{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu x}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 z}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-6 \right) \right] \hfill\cr G_{21} = \displaystyle{{3\mu xy}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 1{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu y}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 x}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-1 \right) \right] \hfill\cr G_{22} = \displaystyle{{\mu (3y^2 -r^2)}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 1{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu y}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 y}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-1 \right) \right] \hfill\cr G_{23} = \displaystyle{{3\mu yz}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 1{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu y}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 z}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-6 \right) \right] \hfill\cr G_{31} = \displaystyle{{3\mu xz}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 4{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu z}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 x}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-3 \right) \right] \hfill\cr G_{32} = \displaystyle{{3\mu yz}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 4{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu z}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 y}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-3 \right) \right] \hfill\cr G_{33} = \displaystyle{{\mu (3z^2 - r^2)}\over{r^5}} \left[ 1 + J_2 \displaystyle{{R_e^2}\over{r^2}}\left( 4{\cdot}5 - 7{\cdot}5 \displaystyle{{z^2}\over{r^2}} \right) \right] - \displaystyle{{\mu z}\over{r^3}} \left[ 3J_2 \displaystyle{{R_e^2 z}\over{r^4}} \left( 10 \displaystyle{{z^2}\over{r^2}}-8 \right) \right] \hfill\cr}} \right.$$
Based on Equations (2), (3), (8), (A1) and (A2), then
$$\left\{ {\matrix{ \delta \dot{{\bi{r}}}^i = \delta {\bi{v}}^i \hfill\cr \delta \dot{{\bi{v}}}^i = c{\bi{C}}_{b}^{i}[ {\bi{f}}^{b} \times \rsqb {\bi{\theta}} + {\bi{G}} ({\bi{r}}^i) \delta {\bi{r}}^i + c{\bi{C}}_b^i \Delta {\bi{f}}^b \hfill\cr \dot{\bi{\theta}} = -[ {\bi{\omega}}_{ib}^{b} \times \rsqb {\bi{\theta}} + \Delta {\bi{\omega}}_{ib}^{b} \hfill\cr \dot{{\bi{b}}}_f = {\bi{0}} \hfill\cr \dot{{\bi{b}}}_{\omega} = {\bi{0}}\hfill\cr}} \right.$$
The matrix form is
$$\left( {\matrix{ {\delta {\mathop {\bi r}\limits^. }^i} \cr {\delta {\mathop {\bi v}\limits^. }^i} \cr {\mathop {\bi{\theta}}\limits^. } \cr {{\mathop {\bi b}\limits^. }_f} \cr {{\mathop {\bi b}\limits^. }_\omega } \cr } } \right) = {\bi A}\left( {\matrix{ {\delta {\bi r}^i} \cr {\delta {\bi v}^i} \cr {\bi{\theta}} \cr {{\bi b}_f} \cr {{\bi b}_\omega } \cr } } \right) + {\bi B}\left( {\matrix{ {\Delta {\bi f}^b} \cr {\Delta {\bi{\omega}}_{ib}^b } } } \right)$$
In Equation (A4):
$${\bi A} = \left( {\matrix{ {\bi 0} & {\bi I} & {\bi 0} & {\bi 0} & {\bi 0} \cr {{\bi G(}{\bi r}^i{\bi )}} & {\bi 0} & {c{\bi C}_b^i [{\bi f}^b \times ]} & {c{\bi C}_b^i } & {\bi 0} \cr {\bi 0} & {\bi 0} & {- [{\bi{\omega}}_{ib}^b \times ]} & {\bi 0} & {\bi I} \cr {\bi 0} & {\bi 0} & {\bi 0} & {\bi 0} & {\bi 0} \cr {\bi 0} & {\bi 0} & {\bi 0} & {\bi 0} & {\bi 0} \cr } } \right), {\bi B} = \left( {\matrix{ {\bi 0} & {\bi 0} \cr {c{\bi C}_b^i } & {\bi 0} \cr {\bi 0} & {\bi I} \cr {\bi 0} & {\bi 0} \cr {\bi 0} & {\bi 0} \cr } } \right)$$
Equation (6) can be derived by discretizing Equation (A4) and retaining the first order small quantity. In Equation (6),
${\bi {\varPhi}}_{k\comma k-1} = {\bi{I}} + A_{t_{k}} \Delta t$
;
${\bi{\varGamma}}_{k} = \displaystyle{{1}\over{2}} \lpar {\bi{I}} + {\bi {\varPhi}}_{k\comma k-1}\rpar {\bi{B}}_{t_{k}} \Delta t$
. Equation (6) is known as the SINS error state equation, whose covariance matrix propagation equation is shown as Equation (7).
APPENDIX B. THE DERIVATION OF APPARENT HEIGHT MEASUREMENT EQUATION
Similar perturbation analysis is conducted to obtain the apparent height measurement equation. SINS outputs vehicle position
$\hat{{\bi{r}}}^{i}$
and attitude matrix
$\hat{{\bi{C}}}_{b}^{i}$
, and the lever-arm vector
$\overline{{\bi{l}}}_{s}$
can be measured accurately in advance. According to Equation (8), the star sensor position is approximated by truncating the Taylor series to its first-order:
$$\hat{{\bi r}}_s^i = \hat{{\bi r}}^i + \hat{{\bi C}}_b^i \overline {\bi l} _s \approx {\bi r}^i + \hat{{\bi C}}_b^i \overline {\bi l} _s + \delta {\bi r}^i - \hat{{\bi C}}_b^i [\overline {\bi l} _s \times ]{\bi{\theta}}$$
Suppose ĥ
a
is the apparent height in error, calculated by vehicle position
$\hat{{\bi{r}}}^{i}$
and attitude matrix
$\hat{{\bi{C}}}_{b}^{i}$
by SINS using Equation (27), the estimated apparent height is
$$\hat{h}_a = \sqrt{(\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)^T (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s) - (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)^T\ {\bi{e}}^{\prime} {\bi{e}}^{\prime T}\ (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)} - R_e$$
Put Equation (B1) into Equation (27) in combination with Equation (B2), the following equation holds true.
$$h_a = \hat{h}_a -\displaystyle{{[(\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s) - {\bi{e}}^{\prime} {\bi{e}}^{\prime T} (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)]^T \left\{ \delta \hat{{\bi{r}}}^i- \hat{{\bi{C}}}_{b}^{i} [ \overline{{\bi{l}}}_{s} \times \rsqb {\bi{\theta}}\right\} } \over{\sqrt{(\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)^T (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s) - (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)^T\ {\bi{e}}^{\prime} {\bi{e}}^{\prime T}\ (\hat{{\bi{r}}}^i + \hat{{\bi{C}}}_b^i \overline{{\bi{l}}}_s)}}}$$
Define
$z_{k}^{h_{a}} = \hat{h}_{a} - \tilde{h}_{a}$
, and put Equation (B3) into Equation (28), then the apparent height observation is
$$\eqalign{z_k^{h_a} &= {\hat h}_a - {\tilde h}_a \cr & = \left( {\sqrt {{({\hat{{\bi r}}}^i + \hat{{\bi C}}_b^i {{\bi \bar l}}_s)}^T({{\bi \hat r}}^i + {\bi \hat C}_b^i {\bi \bar l}_s) - {({\hat {\bi r}}^i + \hat {\bi C}_b^i {\bar {\bi l}}_s)}^T{\bi e}^{\prime}{\bi e}^{\prime T} ({\hat {\bi r}}^i + \hat {\bi C}_b^i {\bar {\bi l}}_s)} - R_e} \right) - {\tilde h}_a \cr & = {\bi M}_r\delta {\hat {\bi r}}^i - {\bi M}_r\hat {\bi C}_b^i [{\bar {\bi l}}_s \times ]{\bi \theta} - \upsilon _a} $$
in which
${\bi{M}}_{{\bi{r}}} = \displaystyle{{[ \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar - {\bi{e}}^{\prime} {\bi{e}}^{\prime T} \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar \rsqb ^{T}}\over{\sqrt{\lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar ^{T} \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar - \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar ^{T}\ {\bi{e}}^{\prime} {\bi{e}}^{\prime T}\ \lpar \hat{{\bi{r}}}^{i} + \hat{{\bi{C}}}_{b}^{i} \overline{{\bi{l}}}_{s}\rpar }}}$
.
The measurement equation is shown in Equation (B4). The measurement design matrix is
$${\bi H}_k^\theta = \left[ {\matrix{ {{\bi M}_r} & {{\bi 0}_{3 \times 3}} & { - {\bi M}_r\hat{{\bi C}}_b^i [{\overline {\bi l} }_s \times ]} & {{\bi 0}_{3 \times 3}} & {{\bi 0}_{3 \times 3}}} } \right]$$
Perturb Equations (28) and (29), then
$$\delta h_a = \displaystyle{{\partial h_a}\over{\partial_{a_r}}} \delta a_r = \left(\sqrt{\displaystyle{{HR_e}\over{2\pi}}} - \displaystyle{{6587{\cdot}501}\over{a_r}} \right) \delta a_r$$
The variance of apparent height observation can be obtained
$$R_k^z = \left(\sqrt{\displaystyle{{HR_e}\over{2\pi}}} - {\displaystyle{{6587{\cdot}501}\over{a_r}}} \right)^2 \sigma_{a_r}^2$$
