Landmark-Based Inertial Navigation System for Autonomous Navigation of Missile Platform

As a new information provider of autonomous navigation, the on-orbit landmark observation offers a new means to improve the accuracy of autonomous positioning and attitude determination. A novel autonomous navigation method based on the landmark observation and the inertial system is designed to achieve the high-accuracy estimation of the missile platform state. In the proposed method, the navigation scheme is constructed first. The implicit observation equation about the deviation of the inertial system output is derived and the Kalman filter is applied to estimate the missile platform state. Moreover, the physical observability of the landmark and the mathematical observability of the navigation system are analyzed. Finally, advantages of the proposed autonomous navigation method are demonstrated through simulations compared with the traditional celestial-inertial navigation system and the deeply integrated celestial-inertial navigation system.


Introduction
The navigation system plays an important role in the actual application of the missile. Inertial Navigation System (INS), Global Navigation Satellite System (GNSS), and Celestial Navigation System (CNS) [1] are the three most commonly used missile navigation systems at present.
INS uses an accelerometer and a gyroscope to measure the line motion and angle motion of the carrier platform. Through the navigation solution, parameters such as speed, position, and attitude of the carrier platform relative to a reference coordinate system can be determined [2]. INS has the advantages of better concealment, stronger autonomy, and higher short-term accuracy. However, the output error of INS accumulates with time. Therefore, INS cannot be used in the missile platform alone.
GNSS can provide rich state information of the carrier platform such as the position and the speed [3]. However, GNSS is semi-autonomous. Influenced by the complex space conditions such as uncertain electromagnetic interference and possible signal occlusion as well as the inherent vulnerability of navigation signals, GNSS's availability and service ability are uncertain to a large extent [4]. It may also be difficult for the receiver to track GNSS signals because of the large Doppler frequency shift of GNSS signals resulting from high dynamic characteristics of the missile platform.
CNS determines the state of the carrier platform by observing celestial bodies and has the advantages of stronger autonomy and concealment. However, the output of CNS is low-frequency and is easily affected by the external environment. Therefore, in view of the CINS's difficulties in the position estimation and advantages of the landmark-based navigation method, the landmark-based method is combined with INS to form an autonomous navigation system for the missile platform. Through the proposed system, named Landmark-based Inertial Navigation System (LINS), the paper attempts to achieve higher accuracy estimation both of the attitude and the position for the missile platform without adding additional sensors. The main contributions of this paper include the following. the mathematical observability of the navigation system-the observability of the proposed system is analyzed. The paper proves that the proposed navigation system is completely observable mathematically when the number of observable landmarks is greater than 1. 3. The realization of the comparative simulation experiments: Compared with the traditional CINS and the deeply integrated CINS, the simulation experiments prove that LINS can greatly improve the accuracy of position estimation while maintaining the attitude estimation accuracy.
It should be pointed out that although the mind of using visual sensors to aid the navigation of INS adopted by this paper is the same as that of other literatures, see, e.g., [23,25], the state transition equation and the observation equation used in this paper are different from those in other articles due to the change of the navigation environment. In the construction of the observation equation, the derivation of equations about error propagations of position and attitude are done by authors. The observability analysis and simulation design given in this paper can also be recognized as innovative.
In view of above research content, the paper will be carried out in the following order. In Section 2, the navigation scheme of LINS is designed. The propagation equation of ballistic error, the implicit observation equation, as well as the state estimation process of the missile platform are given. Section 3 analyzes the physical observability of landmarks and the mathematical observability of the proposed navigation system. In Section 4, advantages of LINS are proved by the comparative simulation experiments. Finally, Section 5 draws our conclusion.

LINS Navigation Scheme and State Estimation Process
In this section, the autonomous navigation scheme of LINS is designed first. Next, the propagation model of the ballistic error, i.e., the state transition equation of the proposed navigation system, is introduced. Then, the implicit observation equation about the INS output deviation is derived. Finally, based on the existing state transition equation and the observation equation, the Kalman filter is applied to the state estimation. Meanwhile, the estimation process of missile platform state is also given.
For the sake of simplicity, the coordinate systems used in the missile navigation are given as follows, the launch-point inertial coordinate frame (li-frame), the missile body coordinate frame (b-frame), and the sensor coordinate frame (s-frame). The specific definition of the coordinate system can be seen in [15]. At the same time, we assume that b-frame and s-frame have the same origin.

Autonomous Navigation Scheme of Missile Platform
As mentioned above, the star sensor used in CINS cannot provide the position information of the carrier platform. Moreover, because of the infinite distance between the missile platform and the star, the position change of the missile has little influence on the observation coordinate of the star. That is to say, the star observation contains little position information of the missile platform. Different from stars, the distance between the landmark and the missile is limited. Both changes of the position and of the attitude can cause great impact on the observation coordinates of the landmark. In other words, the landmark observation contains more abundant state information of the missile platform than the star observation. Therefore, the landmark observation may provide a new means to improve the accuracy of autonomous orbit determination as a novel information source of autonomous navigation.
Based on the above basic idea, the navigation scheme of LINS is constructed in Figure 1 to get the high-accuracy estimation both of the position and the attitude for the missile platform.   As shown in Figure 1, the autonomous navigation scheme obtains coordinates of landmarks with known positions through the landmark matching, and also gets the coordinate estimation of landmarks with INS output. Based on the difference between the landmark observation and the landmark estimation, the implicit observation equation of the INS output deviation can be established. Combined with the ballistic error propagation model, the Kalman filter is applied to the estimation of the INS output error. Furthermore, by the feeding the deviation estimation back to the INS output, the missile platform state is finally estimated.

Ballistic Error Propagation Equation
In this paper, we select li-frame as the navigation frame. All of the following variables are expressed in this frame. Take T is the misalignment angle of the missile platform, i.e., the attitude estimation error of the missile platform; T are the estimation errors of the speed and the position for the missile platform, respectively; ε(t) = [ε x (t), ε y (t), ε z (t)] T represents the angle drift resulting from gyro bias; and The ballistic error propagation equation is as follows, where F(t) is the process input matrix and where I 3 is the unit matrix with the dimension 3 × 3 and C li b is the rotation matrix from b-frame to li-frame. G(t) is the noise drive matrix. Specific expressions of F a (t), F b (t) and G(t) can be found in [26].
Discretize Equation (1), the state transition equation of the navigation system can be obtained as follows, where the subscript k represents the time, When the discrete time step is T, we have where I 15 is the unit matrix with the dimension 15 × 15. It is generally assumed that the system noise, T , obeys the Gaussian distribution with zero mean value and the covariance matrix of Q k .

Landmark Observation Equation
The effective extraction, matching, and tracking of the landmarks based on the earth image are the preconditions for the construction of the observation equation. The progress in the following aspects paves the way for the application of the proposed navigation method. First, with the increase of the number of satellites, more and more high-precision earth images can be obtained, which means that more landmarks can be observed from the missile platform. Next, there has been a variety of methods concentrating on image matching such as Harris detector [27], Scale-invariant feature transform (SIFT) algorithm [28], and speeded up robust features (SURF) algorithm [29]. These algorithms can basically achieve the fast identification of the landmark and resist the influence of light change, mist interference and other environmental changes. Finally, with the increase of computer computing speed, it has been possible to perform feature matching in real time. For example, in [30], GPU-based implementation of SURF is able to extract and match features from images with 640 × 480 resolution at 103 frames per second. In [31], an efficient FPGA-based implementation of SURF is developed to process images with 800 × 600 resolution at 60 frames per second.
Under the support of the above developed technologies, we establish the observation model as follows. First of all, the observation diagram of the landmark is given in Figure 2.
As Figure 2 shows, if landmark-i with known coordinate ρ i (expressed in li-frame) is caught through the image recognition and matching, the following equation can be derived from the geometric relationship, where p i k (expressed in s-frame) denotes the vector from the missile platform to landmark-i. Three axes of s-frame, X S , Y s , and Z s , are shown in Figure 2, r k represents the position of the missile platform at time k. C b s is the rotation matrix from s-frame to b-frame, which is related to the installation of the imaging sensor. C li b,k is the rotation matrix from b-frame to li-frame at time k expressed as follows, where ϕ k , ψ k , and γ k denote the pitch, yaw, and roll angles of the missile platform, respectively.  Equation (9) can be derived from Equation (7).

计理论
where C b li is the rotation matrix from li-frame to b-frame and C s b is the rotation matrix from b-frame to s-frame, and we also have Assume that the theoretical imaging coordinate of landmark-i is [x i k , y i k ], then the following equation holds, where p i k,x , p i k,y , p i k,z are three components of p i k and f is the focal length of the imaging sensor. The above derivation proves that the imaging coordinate of landmark-i can be determined by its location and the position and attitude of the missile platform when the installation matrix of the landmark sensor, i.e., C b s is known. However, the real state of the missile platform is unknown. However, the position outputr k and the attitude output ζ k = [φ k ,ψ k ,γ k ] T of INS can be used to obtain the estimation of p i k ,p i k . The expression ofp i k and the corresponding estimation error, δp i k , can be given as follows, whereĈ b li,k is the rotation matrix from li-frame to b-frame corresponding to ζ k , δr k = [δx k , δy k , δz k ] T is the estimation error of the missile platform position. For the vector ω = ω x , ω y , ω z T , lines 1, 2, and 3 of h(ζ k , ω) are as follows, respectively.
The derivation of h(ζ k , ω) is given in Appendix A. In Equation (14), θ k = [δϕ k , δψ k , δγ k ] T is the attitude error angle resulted from the misalignment angle outputted by gyro. The transition equation from φ k to θ k is as follows, The detailed derivation of Θ k can be found in [32]. According top i k and its estimation error δp i k , the imaging coordinate estimation of landmark-i,ẑ i k , and its relevant estimation error, δz i k , can be given as follows, where I 2 is the unit matrix with the dimension 2 × 2. Further, the observation of landmark-i can be expressed as Equation (22).
where v i k is the observation noise and it is usually considered to obey Gaussian distribution with zero mean value and covariance matrix of R i k . Based on the above equation, the implicit observation equation can be constructed as follows, where So far, the imaging coordinate estimation of landmark-i,ẑ i k , has been derived through the position and attitude outputted by INS. Furthermore, the observation equation about INS output deviation, θ k and δr k , has been established by subtractingẑ i k from z i k .

State Estimation Process of Missile Platform
The ballistic error propagation model and the observation equation have been obtained in Sections 2.2 and 2.3, respectively. Then, the filter algorithm of LINS can be given as follows by applying Kalman filter to the state estimation for the missile platform. The filter algorithm is as follows.
STEP1: According to the state estimation at time k − 1,X k−1 , and the corresponding estimation error covariance matrix P k−1 , calculate the state prediction,X k,k−1 , and P k,k−1 through the following equation.X STPE2: Assume that n landmarks are matched at time k, the implicit observation equation can be obtained by computing Equations (28)- (32).
STEP3: CalculateX k and corresponding P k through Equations (33)- (37). Estimate the missile platform state by compensatingX k to the INS output. Set k = k + 1 and return to STEP1.

Observability Analysis
The observability is the premise of the effective implementation for the navigation scheme and the filtering algorithm. Therefore, in this section, the observability of the proposed navigation system is analyzed from two aspects, the physical observability and the mathematical observability. The so-called physical observability refers to the visibility of the landmark to the missile. The mathematical observability reflects the ability of the navigation system to achieve the optimal estimation of the system state by combining the measurements with the system state model [33].

Physical Observability
In order to make the landmark visible to the landmark sensor installed on the missile platform, the conditions shown in Figure 3 should be satisfied. First, the landmark should be within the horizon of the missile position (seen as Figure 3a). Second, the landmark should be located in the view field of the landmark sensor (seen as Figure 3b).  In Figure 3a, ρ e i and r e k are the position vectors of landmark-i and the missile platform expressed in the earth fixed coordinate frame, respectively. R denotes the earth radius. In order to satisfy the physical condition, the following formula should hold.
where · denotes the 2-norm of a vector.
Here, we assume that the earth is regarded as a standard sphere, which may lead to the erroneous judgement of the observability. For the sake of safety, R can be taken as the shortest distance from the center of the earth to its surface. Figure 3b shows that the landmark should be in the view field of the landmark sensor. If the field angle of the landmark sensor is 2α, then in order to meet the field condition, the following equation should be satisfied, In addition to the two conditions shown in Figure 3, the landmark may not be observed due to the absence of visible light. This means that the proposed navigation system may only be applicable to missiles launched in the daytime. However, it should be noted that due to the strong radiation of the atmospheric background during the day, the difficulty of the CINS in the star observation limits its application in the daytime launched missiles [34,35]. The proposed system may be an alternative to the CINS under the strong visible light. In addition, the number of observable landmarks may be reduced when the missile passes through the desert, ocean, and areas covered by clouds or fog. However, the following simulation shows that even though the number of matched landmarks is small, the LINS can still achieve high-accuracy state estimation.

Mathematical Observability
For the sake of simplicity, assume that only one landmark is observed, and then the mathematical observability matrix [36] of LINS can be given as follows, where The symbols used in Equations (40)-(42) have been defined above. The fact that the time subscript k is omitted means that symbols here are independent from time. Then Obviously, HΦ has full column rank, so M also has full column rank. Therefore, the proposed navigation system is fully observable when the number of observable landmarks is not less than 1.

Simulation
In order to verify the advantages of LINS, the traditional CINS and the deeply integrated CINS proposed in [15] are used as the control group of the comparative simulation experiment. Parameter settings of the missile trajectory, the landmark sensor and the INS output are shown in Tables 1-3, in which settings of the ballistic parameter and INS output parameter have referred to [37].   Table 3. Parameters of Inertial Navigation System (INS) output. In addition, set the attitude estimation accuracy of the star sensor used in CINS as 8 arc seconds, the measurement accuracy of the altimeter in the deeply integrated CINS as 50 m and the rotation matrix from s-frame to b-frame as follows,

Gravity Acceleration
The filter periods of the three navigation systems, i.e., the time intervals between two adjacent state estimation of the filter, are set to 0.1 s. Then, 200 landmarks are randomly generated in the region with the longitude ∈ [116. 34,188.57] and the latitude ∈ [14.028, 57.169]. In order to test the state estimation performance of the proposed algorithm under different number of measurements, the following two cases are considered.
Case 1: LINS can apply the landmarks to the state estimation as long as they are observed. Case 2: Up to three observable landmarks are randomly selected and used for missile state estimation Case 1 is a simulation of the actual navigation scene, whereas Case 2 tests the performance of LINS when the number of observable landmarks is small. It should be noted that in Case 2, LINS is provided with the same or less measurements as the deeply integrated CINS which requires at least two stars to obtain the attitude estimation and additional altitude observations to estimate the missile position. Figure 4 shows the generated trajectory of the missile in the earth inertial frame, the randomly generated landmarks (represented by green pentagram), and the observed landmarks (represented by red pentagram). Figure 5 displays the observation episodes of the landmarks. When landmarks-i is observed at time k, the corresponding location is marked with the real point, otherwise there is no mark. Figure 6 illustrates the imaging track of the observable landmarks constituted composed of imaging coordinates at different time.   Figure 7 gives the number of landmarks that can be used for the state estimation of the missile platform under two cases. From Figure 7, it can be seen that the number of the observable landmarks is small or even zero during the early and late flight stages due to the limited flight altitude and the landmark density. When the number of the observable landmarks is 0, the proposed navigation system cannot work and the system output is consistent with that of the INS. In the practical application, the suitable storage of the geodetic image may avoid the situation of too few visible landmarks. Meanwhile, because of the number limitation, under Case 2, the maximum number of landmarks that can be used for state estimation is 3.   Figures 8 and 9 show the position and attitude estimation errors given by CINS of two different combination modes and by LINS under two cases with different number of usable landmarks. As seen from Figures 7 and 8, the position estimation accuracy of the proposed navigation system increases rapidly with the number of observable landmarks rising. Even when the maximum number of landmarks available for state estimation is 3, LINS outperforms CINS in the position estimation of the missile platform. However, when the number is not limited, LINS behaves more stable in position estimation.
Similarly, according to Figures 7 and 9, it can be seen that the accuracy of attitude estimation obtained by LINS increases when the number of observable landmarks rises. LINS performs satisfactory in attitude estimation even when the maximum number of available landmarks is 3.  Table 4 compares the root mean square error (RMSE) of the position and attitude estimation given by CINS of two combination modes and LISN under two cases of different number of usable landmarks. It should be noted that datas in Table 4 are the average results of 100 Monte Carlo experiments when the number of observable landmarks exceeds 3. As seen from the table, LINS performs better than CINS in the position estimation even when the maximum number of landmarks available for position estimation is reduced to 3. Under Case 2, the position estimation error of LINS is 80.90% and 61.55% lower than that of the traditional CINS and the deeply integrated CINS, respectively. It means that LINS performs more satisfactory than CINS in the position estimation of the missile platform. In addition, in the attitude estimation, the proposed system is slightly inferior to the CINS, but its attitude estimation accuracy remains high. Under two cases of different number of available landmarks, the average RMSE of attitude estimation that LINS obtains by 100 simulation experiments is 2.90 arc second and 1.67 arc second, respectively.

Conclusions
In order to deal with difficulties of CINS in the position estimation, LINS is designed in this paper. In the proposed method, the implicit observation equation of the INS output deviation is derived firstly. Next, the physical observability of the landmark and the mathematical observability of the LINS are analyzed. Theoretical analysis shows that the proposed navigation system is fully observable in mathematics. Compared with the traditional CINS and the deeply integrated CINS, the simulation results demonstrate that position estimation RMSE of the proposed navigation method is 80.90% and 61.55% lower than that of the other two systems, respectively, even when the maximum number of landmarks that can be used for state estimation is 3. Although LINS does not perform as excellent as the other two navigation methods in the attitude estimation, its estimation accuracy still maintains high.
It should be pointed out that the influence of the atmospheric refraction, the aerodynamics, etc. on the accuracy of landmark observation is not considered in this paper. In the future research, these observation errors will be dealt with. In addition, the number of observable landmarks may be small under the condition that the missile passes through the ocean, the desert, and areas covered by clouds or fog. The estimation of the missile platform with smaller number of observable landmarks needs also to be studied in the future.