SINS/Landmark Integrated Navigation Based on Landmark Attitude Determination

Abstract

Based on the situation that the traditional SINS (strapdown inertial navigation system)/CNS (celestial navigation system) integrated navigation system fails to realize all-day and all-weather navigation, this paper proposes a SINS/Landmark integrated navigation method based on landmark attitude determination to solve this problem. This integrated navigation system takes SINS as the basic scheme and uses landmark navigation to correct the error of SINS. The way of the attitude determination is to use the landmark information photographed by the landmark camera to complete feature matching. The principle of the landmark navigation and the process of attitude determination are discussed, and the feasibility of landmark attitude determination is analyzed, including the orthogonality of the attitude transform matrix, as well as the influences of the factors such as quantity and geometric position of landmarks. On this basis, the paper constructs the equations of the SINS/Landmark integrated navigation system, testifies the effectiveness of landmark attitude determination on the integrated navigation by Kalman filter, and improves the navigation precision of the system.

1. Introduction

In view of the advantages of strong autonomy, anti-interference and good concealment, SINS and CNS are widely used [1,2]. Inertial devices can provide the navigation data of attitude, velocity and position. Star sensors measure the information of astronomical object. This information can be combined and used to obtain more precise navigation results [3,4,5,6]. However, in real application, CNS is unable to work continuously throughout the day. In addition, it can be affected by bad weather, thus SINS/CNS integrated navigation cannot realize a continual navigation for a long time [7].

Recently, as a new navigation technology, landmark navigation has received wide attention globally. As can be seen, landmark navigation is technically different from CNS. It is based on the visualization of the landmarks, which can solve the drawbacks of star sensors and navigate the aircraft by using the geographical features of the planet’s surface [8,9]. To compensate the application limitation, this paper combines SINS with landmark navigation to construct a pattern with high precision to meet navigation requirements.

Currently, some studies have been conducted on the theory and application of landmark navigation. For the single landmark navigation, Costello and Castro [10] extracted landmark characteristics from 2D sensor images, and combined them with the landmark database to estimate the latitude, longitude and attitude of the vehicles on the ground or targets at space. Cesetti et al. [11] proposes a vision-based method of applying UAV (unmanned aerial vehicle) for landing. The basic principle of the guiding method is to define the target area by satellite images or antenna with high resolution . The key of the navigation strategy is to locate proper natural landmarks by feature matching algorithm.

For the applications of landmark navigation in practice, He et al. [12] proposed a method to obtain the information of the space target emitter by landmarks. The purpose is to obtain the azimuth angle of the target, and then calculate the attitude by planarization algorithm and linearization technique. In Khuller et al. [13], landmark navigation is applied in the field of robot industry. Robots can realize the position determination through the landmarks based on the direction information provided by visual inspection. It proposes the minimum set of landmarks for the robot position determination, and the quantity of this minimum set of landmarks is called “measurement dimension”.

However, landmark navigation is also limited by the weather and environment conditions, thus it should be integrated with other navigation methods to realize a more precious navigation. In the integration algorithm between the landmark navigation and other autonomous navigation methods, Kim and Hwang [14] raised the integration of SINS and landmark navigation. Its navigation mode is roughly the same as the one in this paper. It is designed to solve the problem of visual navigation under poor sight. When the environment makes it hard to observe landmarks, the landmark visual navigation is forced to interrupt; the inertial devices continue working and providing position, velocity and attitude information. This method can make up the shortfalls of single landmark navigation and realize a continuous navigation.

When the landmark navigation is combined with other non-autonomous navigations, Babel [15] proposed a navigation algorithm to determine the shortest route of UAV(unmanned aerial vehicle). The landmark navigation is used to compensate the navigation interruption of the GPS. Meanwhile, the method regularly updates the landmark base to realize long-term landmark navigation.

The above schemes all use landmarks for position determination, and the precision of their navigation results can be improved greatly. Under the practical research background, both the aircraft and the astronomical object are relatively rotating to the inertial system, thus it is hard to identify the real attitude of the aircraft to the fixed coordinate system of astronomical object. To achieve the best navigation result, the landmark information should be fully used to accomplish the attitude determination. Therefore, the key of the landmark navigation is to determine the attitude based on landmark characteristics.

In view of the autonomous navigation around the Earth, this paper proposes a SINS/Landmark integrated navigation method with high precision based on landmark attitude determination to meet the demands of the all-day and all-weather autonomous navigation. During the landmark navigation, landmark camera photographs the landmarks at its sampling frequency to capture the landmark characteristics. Then, the characteristics are recognized and matched with the landmark database to obtain the position and attitude information of the aircraft. Position and attitude information by the landmark navigation is combined with the measurement by inertial devices to get more precise navigation results. This paper also introduces the landmark navigation’s principle and attitude determination process, constructs the equations for SINS/Landmark integrated navigation system, compares the navigation results with those have non-attitude determination in the simulations, and testifies the effectiveness of landmark attitude determination on integrated navigation.

The main contributions of this paper include the following. (1) Combine landmark navigation with SINS: This paper proposes SINS/Landmark integrated navigation mode, which can continue navigating and realizing an all-day and all-weather navigation when the SINS/CNS mode is not working. (2) This method uses the landmark features obtained by the landmark camera to accomplish attitude determination, and combines the attitude information with the measurement of SINS to realize an integrated navigation with high precision. (3) This method applies Kalman filter to the linear navigation system, and illustrates the influence of the attitude determination on the navigation accuracy by comparison in the simulations.

The remainder of the paper is structured as follows. Section 2 introduces the principle of landmark navigation, including the acquisition and matching of landmark information. Section 3 introduces the process of determining the attitude by using the landmarks. In this process, the computability of the rotation matrix is verified, and the feasibility of the landmark attitude determination is theoretically explained. Section 4 introduces the SINS/Landmark integrated navigation and gives the system equations. In Section 5, the effectiveness of the SINS/Landmark integrated navigation with landmark attitude determination is verified by experimental simulation. Section 6 is the conclusion.

2. Principles of Landmark Navigation

2.1. Acquisition of Landmark Information

The landmark information can be obtained from the landmark camera, and the operation process is mainly divided into two parts: the shooting process of the landmark camera and the matching process of the landmark feature database.

It is known that the imaging principle of the camera is “small hole imaging”, and the landmark position vector obtained by the landmark information is actually the mapping point of the camera imaging focus, which is similar to the star sensor. However, in practical applications, the feature points of the mapping area may not be obvious and easy to extract. Therefore, the camera chooses the landmarks with clear features as the selection set, which is used to replace the virtual landmark of the mapping point. As for the equations of the position and attitude determination, there is no difference between the two selections.

As shown in Figure 1, when the aircraft (navigation target) orbits around the Earth, the landmark camera first captures the landmark images, and the images are transmitted to the information processing module. Then, the features are compared and analyzed, including the position information of the landmarks on the Earth, and then follows up. To keep the navigation going, the duration of the matching process should be less than the shooting time of the camera.

As shown in Figure 1, there are the processes of shooting, landmark capture and landmark information acquisition, respectively.

(a) Firstly, shoot the landmarks by the landmark camera. It should be noted that only one photo can be taken at a time depending on the shooting frequency of the camera.

(b) Secondly, the landmark camera obtains the images of the landmarks. At this time, multiple landmarks and landmark feature points are obtained according to the photos, and these feature points participate in the next step for matching.

(c) Finally, the landmark feature points in the photo are extracted and then matched with the feature points database. When the matching is completed, the aircraft’s space model is established for navigation. The space model is shown in the third picture of Figure 1. Since the vectors in the figure are all referenced to the Earth-centered-Earth-fixed system, the superscript of the vectors e, ${\mathit{\rho }}_1^e$ and ${\mathit{\rho }}_2^e$ are the position coordinates of the landmarks; ${\mathit{\rho }}_0^e$ is the vector from Landmark 1 to Landmark 2; ${\mathit{r}}^e$ is the position coordinates of the aircraft; and ${\mathit{r}}_1^e$ and ${\mathit{r}}_2^e$ are, respectively, the vectors from Landmark 1 and Landmark 2 to the aircraft, i.e., the relative position model of the aircraft and landmarks at space.

When the aircraft is not parallel to the landmark flat, the captured image of the landmark is prone to rotation and deformation as shown IN Figure 2:

When the landmark camera is not parallel to the landmark flat, the segment $ab$ of the landmark is imaged as g on the imaging area of the camera. According to the principle that the proportion of the similar triangle is constant, the imaging of the segment $ab$ in the virtual flat should be h, and then the image compresses in the $\mathit{O}$ direction, the compression proportion $\kappa$ is as Equation (1):

$$\kappa =\frac{g}{h}$$

(1)

When the landmark camera is fixed on the aircraft, the transformation matrix between the landmark camera coordinate system and the aircraft coordinate system is a constant matrix. When the landmark camera changes direction with the flight of the aircraft, the transformation matrix will also be changed. In practice, the transformation matrix needs to be selected and calculated according to the situation.

2.2. Matching Process of the Landmark Images

The image rotation and deformation should be considered when matching the landmarks, which will take more time. In practice, this process can be operated by using sparse representation method or feature point detection method.

The sparse representation method is to express few original signals to the total by using a linear combination of these original signals [16,17,18], but it takes a long time, which cannot meet the real-time requirement. The feature point detection method is widely used in the visual-based celestial landing navigation. SIFT (Scale-Invariant Feature Transform) is one of the popular methods with preferable application effect [19,20], which realizes the identification and matching of landmark images by key point detection, description, matching and elimination of mismatch points. This algorithm has good discriminability, strong scalability and better robustness in the case of target rotation and deformation, and it can extract multiple features of the target.

For the matching process of landmarks, the operational process of the SIFT is shown in Figure 3:

The scale space theory is used to search the feature points [21], and the Gaussian pyramid [22] and the Gaussian difference pyramid need to be established to compare with the pixels. The gradient of neighborhood feature points needs to be calculated and the main direction leads to the peak. The KD tree [23] is used for the matching process and the threshold should be set by the circumstances.

3. Position and Attitude Determination of the Landmark Navigation

3.1. Attitude Determination of the Landmark Navigation

3.1.1. Calculation for Attitude Angle

The coordinate systems used in this paper are as follows: i is the inertial coordinate system; e is the Earth-centered-Earth-fixed system; b is the aircraft body coordinate system; and $li$ is the launching inertial system.

Assume that the position of the aircraft is ${\mathit{r}}^e=\left(x^e,y^e,z^e\right)$ in the Earth-centered-Earth-fixed system, the position of the observed landmark is ${\mathit{\rho }}_i^e=\left(a_i^e,b_i^e,c_i^e\right)i=1,\cdots ,\tau$, representing that $\tau$ landmark can be observed. Assume that the distance between the aircraft and each landmark point is $r_i$, and $\left({\alpha }_i,{\beta }_i,{\eta }_i\right)$ is the angle between the axes of the aircraft body coordinate system and the vector from the aircraft to the landmark. ${\mathit{C}}_e^b$ is the transformation matrix from the Earth-centered-Earth-fixed system to the aircraft body coordinate system, then:

$${\mathit{C}}_e^b{\left[\begin{array}{ccc}\frac{x^e-a_i^e}{r_i}& \frac{y^e-b_i^e}{r_i}& \frac{z^e-c_i^e}{r_i}\end{array}\right]}^{\mathrm{T}}=\left[\begin{array}{c}\cos {\alpha }_i\\ \cos {\beta }_i\\ \cos {\eta }_i\end{array}\right]$$

(2)

Let

$${\left[\begin{array}{ccc}\frac{x^e-a_i^e}{r_i}& \frac{y^e-b_i^e}{r_i}& \frac{z^e-c_i^e}{r_i}\end{array}\right]}^{\mathrm{T}}={\mathit{X}}_i,\left[\begin{array}{c}\cos {\alpha }_i\\ \cos {\beta }_i\\ \cos {\eta }_i\end{array}\right]={\mathit{Z}}_i$$

(3)

Because of the measurement error, according to Equation (2), it can be written as Equation (4):

$$\mathit{Z}={\mathit{C}}_e^b\mathit{X}+\mathit{\epsilon }$$

(4)

where $\mathit{X}={\left[{\mathit{X}}_1,\cdots {\mathit{X}}_{\tau }\right]}^{\mathrm{T}}$, $\mathit{Z}={\left[{\mathit{Z}}_1,\cdots {\mathit{Z}}_{\tau }\right]}^{\mathrm{T}}$, $\mathit{\epsilon }={\left[{\epsilon }_1,\cdots {\epsilon }_{\tau }\right]}^{\mathrm{T}}\sim N\left(\mathbf{0},{\mathit{\sigma }}^2\right)$. If $\tau \ge 3$, the transformation matrix can be calculated by the least squares (LS) method as Equation (5):

$${\mathit{C}}_e^b=\mathit{Z}{\mathit{X}}^{\mathrm{T}}{\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}\right)}^{-1}$$

(5)

The Launch point’s coordinate in the geodetic coordinate system is $\left(B,L,H\right)$, and the angle of incidence is A, then the transformation matrix between the launch inertial system and the geocentric inertial system can be calculated as Equation (6):

$${\mathit{C}}_{li}^i=\left[\begin{array}{ccc}\sin L& \cos L& 0\\ -\cos L& \sin L& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos B& -\sin B\\ 0& \sin B& \cos B\end{array}\right]\left[\begin{array}{ccc}-\sin A& 0& -\cos A\\ 0& 1& 0\\ \cos A& 0& -\sin A\end{array}\right]$$

(6)

The transformation matrix between the geocentric inertial system and the Earth-centered-Earth-fixed system is

$${\mathit{C}}_i^e=\left[\begin{array}{ccc}\cos \left(\omega T\right)& \sin \left(\omega T\right)& 0\\ -\sin \left(\omega T\right)& \cos \left(\omega T\right)& 0\\ 0& 0& 1\end{array}\right]$$

(7)

where $\omega$ is the angular velocity rate of the Earth’s rotation, and T is the rotation time of the Earth. At this point, the transformation matrix between the launch inertial system and the aircraft body coordinate system can be obtained as Equation (8):

$${\mathit{C}}_{li}^b={\mathit{C}}_e^b{\mathit{C}}_i^e{\mathit{C}}_{li}^i$$

(8)

Then, the pitch angle, yaw angle and roll angle of the aircraft can be obtained, separately.

$$\left\{\begin{array}{c}\phi =\arctan \left({\left({\mathit{C}}_b^{li}\right)}_{2,1}/\phantom{{\left({\mathit{C}}_b^{li}\right)}_{2,1}{\left({\mathit{C}}_b^{li}\right)}_{1,1}}{\left({\mathit{C}}_b^{li}\right)}_{1,1}\right)\hfill \\ \varphi =-arcsin\left({\left({\mathit{C}}_b^{li}\right)}_{3,1}\right)\hfill \\ \gamma =\arctan \left({\left({\mathit{C}}_b^{li}\right)}_{3,2}/\phantom{{\left({\mathit{C}}_b^{li}\right)}_{3,2}{\left({\mathit{C}}_b^{li}\right)}_{3,3}}{\left({\mathit{C}}_b^{li}\right)}_{3,3}\right)\hfill \end{array}\right.$$

(9)

${\mathit{C}}_b^{li}$ is the transposition of ${\mathit{C}}_{li}^b$, representing the transformation matrix from the aircraft body coordinate system to the launch inertial system.

3.1.2. Computability of the Transformation Matrix

It is necessary to prove the computability of the transformation matrix, but its orthogonal should be proved first. In theory, the transformation matrix is orthogonal obviously, representing the rotation relationship between the two coordinate systems. However, in this paper, the transformation matrix ${\mathit{C}}_e^b$ is obtained by the distance scalar $r_i$ of the aircraft and each landmark point, as well as the angle $\left({\alpha }_i,{\beta }_i,{\eta }_i\right)$ between the coordinate axes of the aircraft body coordinate system, and the vector from the aircraft to the landmark, thus it is significant to prove its orthogonality.

The computability of the transformation matrix between the launch inertial system and the aircraft body coordinate system is equivalent to that of the transformation matrix ${\mathit{C}}_e^b$ from the Earth-centered-Earth-fixed system to the aircraft body coordinate system. That is, Equation (5) can be calculated.

According to Equation (5), the matrix $\mathit{X}$ must be full row rank, that is, the vector from the aircraft to the landmark point can be expanded into the entire space.

Proof for the orthogonality of matrix ${\mathit{C}}_e^b$:

(1) Invariance of the mold length: Let $r_i$ be the mold length of ${\left[\begin{array}{ccc}{x}^e-a_i^e& y^e-b_i^e& z^e-c_i^e\end{array}\right]}^{\mathrm{T}}$, and it can be known that the mold length of the column vectors in matrix $\mathit{X}$ are all 1, and $\left({\alpha }_i,{\beta }_i,{\eta }_i\right)$ is the angle between the three axes of the aircraft body coordinate system and the vector from the aircraft to the landmark. The column vector mold length of $\mathit{Z}$ is also 1.

(2) Invariability of the angles: For any two landmarks (set to be the ith and the jth), the vectors from aircraft to the two landmarks can be set as ${\mathit{r}}_i^e$ and ${\mathit{r}}_j^e$ of the Earth-centered-Earth-fixed system, respectively. ${\mathit{r}}_i^b$ and ${\mathit{r}}_j^b$ are, respectively, the two landmarks of the aircraft body coordinate system, satisfying

$$\left\{\begin{array}{c}{\mathit{r}}_i^b={\mathit{C}}_e^b{\mathit{r}}_i^e\hfill \\ {\mathit{r}}_j^b={\mathit{C}}_e^b{\mathit{r}}_j^e\hfill \end{array}\right.$$

(10)

It is known that the same angle does not change in size at different coordinates, and consider that

$$\left\{\begin{array}{c}{\mathit{r}}_i^e{\mathit{r}}_j^e=\left|{\mathit{r}}_i^e\right|\left|{\mathit{r}}_j^e\right|\cos 〈{\mathit{r}}_i^e,{\mathit{r}}_j^e〉=\cos 〈{\mathit{r}}_i^e,{\mathit{r}}_j^e〉\hfill \\ {\mathit{r}}_i^b{\mathit{r}}_j^b=\left|{\mathit{r}}_i^b\right|\left|{\mathit{r}}_j^b\right|\cos 〈{\mathit{r}}_i^b,{\mathit{r}}_j^b〉=\cos 〈{\mathit{r}}_i^b,{\mathit{r}}_j^b〉\hfill \end{array}\right.$$

(11)

Then,

$${\mathit{r}}_i^e{\mathit{r}}_j^e={\mathit{r}}_i^b{\mathit{r}}_j^b$$

(12)

For convenience, taking three sets of linear independent column vectors in two coordinate systems, Equation (4) can be rewritten as Equation (13):

$${\mathit{C}}_e^b\tilde{\mathit{X}}=\tilde{\mathit{Z}}$$

(13)

where $\tilde{\mathit{X}}={\left[{\mathit{r}}_{k_1}^e,{\mathit{r}}_{k_2}^e,{\mathit{r}}_{k_3}^e\right]}^{\mathrm{T}}$, $\tilde{\mathit{Z}}={\left[{\mathit{r}}_{k_1}^b,{\mathit{r}}_{k_2}^b,{\mathit{r}}_{k_3}^b\right]}^{\mathrm{T}}$, $k_1,k_2,k_3$ are three randomly selected landmarks. According to Equation (12),

$${\tilde{\mathit{X}}}^{\mathrm{T}}\tilde{\mathit{X}}={\left({\mathit{r}}_{k_i}^e·{\mathit{r}}_{k_j}^e\right)}_{\genfrac{}{}{0pt}{}{i=1,2,3\hfill }{j=1,2,3\hfill }}={\left({\mathit{r}}_{k_i}^b·{\mathit{r}}_{k_j}^b\right)}_{\genfrac{}{}{0pt}{}{i=1,2,3\hfill }{j=1,2,3\hfill }}={\tilde{\mathit{Z}}}^{\mathrm{T}}\tilde{\mathit{Z}}$$

(14)

Therefore,

$$\tilde{\mathit{Z}}{\tilde{\mathit{X}}}^{-1}={\left({\tilde{\mathit{Z}}}^{-1}\right)}^{\mathrm{T}}{\tilde{\mathit{X}}}^{\mathrm{T}}$$

(15)

Calculating the inverse matrix of ${\mathit{C}}_e^b$, Equation (16) can be obtained after transposition:

$${\left[{\left({\mathit{C}}_e^b\right)}^{-1}\right]}^{\mathrm{T}}={\left({\tilde{\mathit{Z}}}^{-1}\right)}^{\mathrm{T}}{\tilde{\mathit{X}}}^{\mathrm{T}}$$

(16)

Considering Equation (13),

$${\mathit{C}}_e^b={\left({\tilde{\mathit{Z}}}^{-1}\right)}^{\mathrm{T}}{\tilde{\mathit{X}}}^{\mathrm{T}}$$

(17)

According to Equations (16) and (17),

$${\left({\mathit{C}}_e^b\right)}^{\mathrm{T}}={\left({\mathit{C}}_e^b\right)}^{-1}$$

(18)

Therefore, the matrix ${\mathit{C}}_e^b$ is orthogonal. Moreover, it is computable.

3.1.3. The Relationship between the Number, Relevance and Accuracy of Landmarks

The selection of landmarks will affect the accuracy of navigation. The influential factors mainly contain the number of landmarks and the relevance of landmark locations (whether they are collinear or not), etc. The two aspects are discussed and the analysis results are given as follows.

From Equation (4), the estimation error of the transformation matrix ${\mathit{C}}_e^b$ is as Equation (19):

$$Cov\left({\mathit{C}}_e^b\right)={\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}\right)}^{-1}{\mathit{\sigma }}^2$$

(19)

The relationship between the number of landmarks and the accuracy:

According to Gorbenko and Popov [24], for the set of landmarks, there is usually a minimum set of them. Meanwhile, the completeness can be satisfied. In the process of the position determination, the purpose is to obtain the coordinate of the aircraft. In general, a three-dimensional coordinate has three unknowns elements, and this means the number of observable landmarks should be three or more. The specific proof and analysis can refer to Section 3.2.

However, for the process of attitude determination, since the solution of the transformation matrix is calculated by the LS method using multiple samples, the relationship between the selection of the landmarks and the accuracy of the attitude needs to be determined according to Equation (19).

The following part is the analysis of the number of selected landmarks in the attitude determination.

Assuming that a new landmark is added on the basis of several original landmarks, Equation (19) can be rewritten as

$$\begin{array}{c}Cov\left({\mathit{C}}_e^b\right)={\left(\left[\begin{array}{cc}\mathit{X}& {\mathit{X}}_{\tau +1}\end{array}\right]\left[\begin{array}{c}{\mathit{X}}^{\mathrm{T}}\\ {\mathit{X}}_{\tau +1}^{\mathrm{T}}\end{array}\right]\right)}^{-1}{\mathit{\sigma }}^2\hfill \\ ={\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}+{\mathit{X}}_{\tau +1}{\mathit{X}}_{\tau +1}^{\mathrm{T}}\right)}^{-1}{\mathit{\sigma }}^2\hfill \end{array}$$

(20)

Since the eigenvalues of the matrix ${\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}+{\mathit{X}}_{\tau +1}{\mathit{X}}_{\tau +1}^{\mathrm{T}}\right)}^{-1}$ are smaller than the eigenvalues of the matrix ${\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}\right)}^{-1}$, within the calculation amount, a larger number of landmarks means higher accuracy. Since the landmark camera can only get one picture at a time according to its shooting frequency, and the number of landmarks in the picture is limited, it can be concluded that the more landmarks there are, the higher is the accuracy of the attitude that can be achieved.

The relationship between the landmark correlation and the accuracy:

If there is a strong correlation between the landmarks, even the landmarks are selected as points near a line, the minimum eigenvalue of the matrix $\mathit{X}{\mathit{X}}^{\mathrm{T}}$ tends to zero. Make an eigenvalue decomposition (ED) of the matrix:

$${\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}\right)}^{-1}=\mathit{P}{\mathbf{\Lambda }}^{-1}{\mathit{P}}^{-1}$$

(21)

Then, the mean square error (MSE) of the transformation matrix ${\mathit{C}}_e^b$ is as Equation (22):

$$\begin{array}{c}MSE\left({\mathit{C}}_e^b\right)={\mathit{\sigma }}^2\mathrm{tr}{\left(\mathit{X}{\mathit{X}}^{\mathrm{T}}\right)}^{-1}={\mathit{\sigma }}^2\mathrm{tr}\left(\mathit{P}{\mathbf{\Lambda }}^{-1}{\mathit{P}}^{-1}\right)\hfill \\ ={\mathit{\sigma }}^2\mathrm{tr}\left({\mathbf{\Lambda }}^{-1}\mathit{P}{\mathit{P}}^{-1}\right)={\mathit{\sigma }}^2\mathrm{tr}\left({\mathbf{\Lambda }}^{-1}\right)\hfill \\ ={\mathit{\sigma }}^2{\displaystyle \sum _{i=1}^3}{\lambda }_i^{-1}\hfill \end{array}$$

(22)

Since the landmarks are considered in three-dimensional space, the eigenvalue matrix is $\mathbf{\Lambda }=\left[\begin{array}{ccc}{\lambda }_1& & \\ & {\lambda }_2& \\ & & {\lambda }_3\end{array}\right]$, and $\mathit{P}$ is the eigenvector matrix.

Since ${\lambda }_3$ approaches 0, when the matrix is inverted, $MSE\left({\mathit{C}}_e^b\right)$ tends to infinity. At this time, the calculation error is particularly large, which affects the calculation accuracy.

The simulation data were used to give an explanation.

Assuming that the measurement angle error obeys the normal distribution with a zero mean value and a standard deviation of 30”, the errors can be calculated under the different number of landmarks and correlation. In each case, Monte Carlo simulation was performed for 100 times to obtain the mean error. The accuracy results are shown in

Table 1. The effect of the number and correlation of landmarks on the accuracy of the transformation matrix.

		3	4	5	6	7
Non-correlation	Pitch angle error	3.4146${}^{″}$	2.0300${}^{″}$	2.3630${}^{″}$	1.6049${}^{″}$	1.8437${}^{″}$
	Yaw angle error	4.5328${}^{″}$	1.4792${}^{″}$	1.9401${}^{″}$	1.8308${}^{″}$	0.6509${}^{″}$
	Roll angle error	1.5083${}^{″}$	2.2644${}^{″}$	0.8618${}^{″}$	0.7915${}^{″}$	0.2807″
Strong correlation	Pitch angle error	28.4667″	24.9020${}^{″}$	13.8017${}^{″}$	11.8040${}^{″}$	3.3160${}^{″}$
	Yaw angle error	7.9635${}^{″}$	2.8863${}^{″}$	4.4863${}^{″}$	4.6482${}^{″}$	4.0131${}^{″}$
	Roll angle error	3.1885${}^{″}$	5.7244${}^{″}$	3.5935${}^{″}$	2.2859${}^{″}$	2.3123${}^{″}$

:

The following can be seen in Table 1:

(a) When there is a strong correlation between the landmarks, the angle error becomes larger, especially the pitch angle error.

(b) Without considering the correlation of the landmarks, more landmarks make the angle error smaller. That is, the more accurate is the transformation matrix, the higher is the accuracy of the attitude determination. This shows that, in the actual navigation process, more landmarks lead to higher accuracy.

(c) In the case that a gradual increase emerges in the number of landmarks, whether there is a correlation relationship between landmarks has less and less influence on the accuracy of the transformation matrix. This shows that, when the number increases, the collinear problem inevitably appears. How to balance the number and the location of the landmarks is the key to improving the navigation accuracy.

3.2. Position Determination of Landmark Navigation

It is shown in Section 3.1 that the position of the ith navigation landmark in the Earth-centered-Earth-fixed system is ${\mathit{\rho }}_i^e=\left(a_i^e,b_i^e,c_i^e\right),i=1,\cdots ,\tau$, the vector of the aircraft under the Earth-centered-Earth-fixed system is ${\mathit{r}}^e=\left(x^e,y^e,z^e\right)$, and the relative position of the aircraft to the landmark is ${\mathit{r}}_i^e$. Let the transformation matrix of the aircraft body coordinate system to the Earth-centered-Earth-fixed system be ${\mathit{C}}_b^e={\left({\mathit{C}}_e^b\right)}^{\mathrm{T}}$, and ${\mathit{C}}_e^b$ can be obtained from Section 3.1.1. Then,

$${\mathit{r}}_i^e={\left(\begin{array}{ccc}{x}^e-a_i^e& y^e-b_i^e& z^e-c_i^e\end{array}\right)}^{\mathrm{T}}$$

(23)

Suppose that ${\mathit{\rho }}_i^b$ is the position vector of the ith landmark in the aircraft body coordinate system, which can be obtained by the landmark camera. Then, it can be obtained as Equation (24):

$$\begin{array}{c}{\mathit{r}}_i^e={\mathit{C}}_b^e{\mathit{\rho }}_i^b\hfill \\ \Rightarrow \left[\begin{array}{c}{x}^e-a_i^e\\ y^e-b_i^e\\ z^e-c_i^e\end{array}\right]={\mathit{C}}_b^e\left[\begin{array}{c}{a}_i^e\\ b_i^e\\ c_i^e\end{array}\right]\hfill \end{array}$$

(24)

Suppose that $x^e,y^e,z^e$ are three unknown vectors, and the coordinate of the aircraft in the Earth-centered-Earth-fixed system can be obtained when there are three or more landmarks can be observed, that is $\tau \ge 3$.

4. SINS/Landmark Integrated Navigation Model

The operation process of SINS/Landmark integrated navigation is based on SINS, and the landmark information is used to correct the error of the SINS. The measurement equation is established according to the position and attitude information obtained by SINS and landmark navigation. The specific navigation process is shown in Figure 4.

4.1. State Equation of Integrated Navigation

Combined with the SINS mathematical platform angle error, velocity error, position error, gyroscope and the accelerometer error models, the state equation of the integrated navigation system can be obtained as Equation (25):

$$\dot{\mathit{X}}\left(t\right)=\mathit{F}\left(t\right)\mathit{X}\left(t\right)+\mathit{G}\left(t\right)\mathit{W}\left(t\right)$$

(25)

Take the state parameter of the system as 15 dimensions, and record it as:

$$\mathit{X}\left(t\right)={\left[{\varphi }_x,{\varphi }_y,{\varphi }_z,\delta v_x,\delta v_y,\delta v_z,\delta x,\delta y,\delta z,{\epsilon }_x,{\epsilon }_y,{\epsilon }_z,{\nabla }_x,{\nabla }_y,{\nabla }_z\right]}^{\mathrm{T}}$$

(26)

where ${\left[\begin{array}{ccc}{\varphi }_x& {\varphi }_y& {\varphi }_z\end{array}\right]}^{\mathrm{T}}$ denotes the three mathematical platform angles error on three axes; ${\left[\begin{array}{ccc}\delta v_x& \delta v_y& \delta v_z\end{array}\right]}^{\mathrm{T}}$ denotes the velocity error; ${\left[\begin{array}{ccc}\delta x& \delta y& \delta z\end{array}\right]}^{\mathrm{T}}$ denotes the position error; $\left[\begin{array}{ccc}{\epsilon }_x& {\epsilon }_y& {\epsilon }_z\end{array}\right]$ denotes the three random constant drift of gyroscope; ${\left[\begin{array}{ccc}{\nabla }_x& {\nabla }_y& {\nabla }_z\end{array}\right]}^{\mathrm{T}}$ denotes the three random constant bias of accelerometer; $\mathit{F}\left(t\right)$ denotes the process input matrix; and $\mathit{G}\left(t\right)$ denotes the process noise matrix.

$$\mathit{F}\left(t\right)={\left[\begin{array}{ccccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{C}}_b^{li}& {\mathbf{0}}_{3\times 3}\\ {\mathit{F}}_b& {\mathbf{0}}_{3\times 3}& {\mathit{F}}_a& {\mathbf{0}}_{3\times 3}& {\mathit{C}}_b^{li}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]}_{15\times 15}$$

(27)

$$\mathit{G}\left(t\right)={\left[\begin{array}{cc}{\mathit{C}}_b^{li}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{C}}_b^{li}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]}_{15\times 6}$$

(28)

where ${\mathit{C}}_b^{li}$ is the transformation matrix from the aircraft body coordinate system to the launch inertial system, and

$${\mathit{C}}_b^{li}=\left[\begin{array}{ccc}\cos \theta \cos \phi & \cos \theta \sin \phi \sin \gamma -\sin \theta \cos \gamma & \cos \theta \sin \phi \cos \gamma +\sin \theta \sin \gamma \\ \sin \theta \cos \phi & \sin \theta \sin \phi \sin \gamma +\cos \theta \cos \gamma & \sin \theta \sin \phi \cos \gamma -\cos \theta \sin \gamma \\ -\sin \phi & \sin \gamma \cos \phi & \cos \gamma \cos \phi \end{array}\right]$$

(29)

where $\phi ,\theta ,\gamma$ are the yaw angle, pitch angle and roll angle of the aircraft measured by the gyroscope, respectively.

${\mathit{F}}_a=\left[\begin{array}{ccc}{f}_{14}& f_{15}& f_{16}\\ f_{24}& f_{25}& f_{26}\\ f_{34}& f_{35}& f_{36}\end{array}\right]$. The parameters $f_{14}$–$f_{36}$ are the derivative of gravitational acceleration on position coordinates, and they vary with the position of the aircraft.

${\mathit{F}}_b=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$. $a_x,a_y,a_z$ are the components of the apparent acceleration along the three axes according to the accelerometer. The process noise of the navigation system is as Equation (30):

$$\mathit{W}\left(t\right)={\left[\begin{array}{cccccc}{w}_{{\epsilon }_x}& w_{{\epsilon }_y}& w_{{\epsilon }_z}& w_{{\nabla }_x}& w_{{\nabla }_y}& w_{{\nabla }_z}\end{array}\right]}^{\mathrm{T}}$$

(30)

where ${\left[\begin{array}{ccc}{w}_{{\epsilon }_x}& w_{{\epsilon }_y}& w_{{\epsilon }_z}\end{array}\right]}^{\mathrm{T}}$ is the random noise of gyro; and ${\left[\begin{array}{ccc}{w}_{{\nabla }_x}& w_{{\nabla }_y}& w_{{\nabla }_z}\end{array}\right]}^{\mathrm{T}}$ is the random noise of accelerometer. The noise covariance matrix of $\mathit{W}\left(t\right)$ is

$$\mathit{Q}\left(t\right)=diag\left(\left[\begin{array}{cccccc}{\sigma }_{{\epsilon }_x}^2& {\sigma }_{{\epsilon }_y}^2& {\sigma }_{{\epsilon }_z}^2& {\sigma }_{{\nabla }_x}^2& {\sigma }_{{\nabla }_y}^2& {\sigma }_{{\nabla }_z}^2\end{array}\right]\right)$$

(31)

4.2. Measurement Equation of Integrated Navigation

According to the landmark navigation process, the output is the attitude and position information of the aircraft, which subtracts the attitude and position information measured from the SINS to obtain the measurement equation. Therefore, the observation of the measurement equation is the transformed platform angle error (the navigation frame misalignment angle) ${\varphi }_x,{\varphi }_y,{\varphi }_z$ and position error $\delta x,\delta y,\delta z$.

Solution process of the mathematical platform misalignment angle ${\varphi }_x,{\varphi }_y,{\varphi }_z$:

In the SINS/Landmark integrated navigation process, SINS obtains the pitch angle ${\theta }_0$, yaw angle ${\phi }_0$ and roll angle ${\gamma }_0$ of the aircraft through the strap down solution, and the landmark camera obtains the pitch angle $\theta$, yaw angle $\phi$ and roll angle $\gamma$, subtracting the two sets of angles to obtain the three-axis attitude error as Equation (32):

$$\Delta \mathit{a}=\left[\begin{array}{c}{a}_{\theta }\\ a_{\phi }\\ a_{\gamma }\end{array}\right]=\left[\begin{array}{c}\theta -{\theta }_0\\ \phi -{\phi }_0\\ \gamma -{\gamma }_0\end{array}\right]$$

(32)

Since the composition of the attitude error equation of SINS is the platform angle error (the navigation frame misalignment angle), it is necessary to convert the attitude error angle of Equation (32) into the platform angle error to establish the measurement equation. The conversion relationship is as Equation (33):

$$\Delta {\mathit{a}}^{\prime }={\mathit{M}}_1\Delta \mathit{a}$$

(33)

where ${\mathit{M}}_1$ is the attitude angle error transformation matrix, and $\Delta {\mathit{a}}^{\prime }={\left[\begin{array}{ccc}{\varphi }_x& {\varphi }_y& {\varphi }_z\end{array}\right]}^{\mathrm{T}}$ is the platform angle error.

Solution process of the position error $\delta x,\delta y,\delta z$:

The position coordinate ${\mathit{r}}^e={\left[\begin{array}{ccc}{x}^e& y^e& z^e\end{array}\right]}^{\mathrm{T}}$ of the aircraft under the Earth-centered-Earth-fixed system can be obtained by the position determination process of landmark navigation as in Section 3.2. The coordinate of the aircraft under the inertial system of the Earth’s core, i.e., ${\mathit{r}}^i={\left(\begin{array}{ccc}{x}^i& y^i& z^i\end{array}\right)}^{\mathrm{T}}$, can be calculated by the inertial device. Then,

$${\tilde{\mathit{r}}}^i={\mathit{C}}_e^i{\mathit{r}}^e$$

(34)

The position error under the inertial system is as Equation (35):

$$\delta {\mathit{r}}^i=\left[\begin{array}{c}\delta x^i\\ \delta y^i\\ \delta z^i\end{array}\right]={\mathit{r}}^i-{\tilde{\mathit{r}}}^i$$

(35)

Converting it to the mathematical platform coordinate system, the conversion relationship is as Equation (36):

$$\delta \mathit{r}={\mathit{M}}_2\delta {\mathit{r}}^i$$

(36)

where ${\mathit{M}}_2$ is the position error conversion matrix, and $\delta \mathit{r}={\left[\begin{array}{ccc}\delta x& \delta y& \delta z\end{array}\right]}^{\mathrm{T}}$ is the position error angle.

Therefore, the measurement equation is as Equation (37):

$$\mathit{Z}\left(t\right)=\left[\begin{array}{c}{\varphi }_x\\ {\varphi }_y\\ {\varphi }_z\\ \delta x\\ \delta y\\ \delta z\end{array}\right]=\mathit{HX}\left(t\right)+\mathit{V}\left(t\right)$$

(37)

where $\mathit{H}={\left[{\mathit{I}}_{3\times 3},{\mathbf{0}}_{3\times 3},{\mathit{I}}_{3\times 3},{\mathbf{0}}_{3\times 3}\right]}^{\mathrm{T}}$, $\mathit{V}={\left[\delta {\Delta }_x,\delta {\Delta }_y,\delta {\Delta }_z,\delta {\xi }_x,\delta {\xi }_y,\delta {\xi }_z\right]}^{\mathrm{T}}$, ${\left[\delta {\Delta }_x,\delta {\Delta }_y,\delta {\Delta }_z\right]}^{\mathrm{T}}$ denotes the difference between the attitude measurement noise of the landmark camera and the constant drift error of the gyroscope. ${\left[\delta {\xi }_x,\delta {\xi }_y,\delta {\xi }_z\right]}^{\mathrm{T}}$ is the difference between position measurement noise of the landmark camera and the constant error of the accelerometer.

4.3. Integrated Navigation Filtering Algorithm

Consider the integrated navigation system according to Equations (25) and (37), discretize it and use Kalman filter to estimate it. The system equation can be transformed as Equation (38):

$$\left\{\begin{array}{c}{\mathit{X}}_{k+1}={\mathbf{\Phi }}_k{\mathit{X}}_k+\mathbf{\Gamma }{\mathit{W}}_k\hfill \\ {\mathit{Z}}_k=\mathit{H}{\mathit{X}}_k+{\mathit{V}}_k\hfill \end{array}\right.$$

(38)

Assuming that T is the sampling interval, then

$$\begin{array}{c}{\mathbf{\Phi }}_k=\mathit{I}+{\mathit{F}}_{k-1}T+\frac{1}{2!}{\mathit{F}}_{k-1}^2{T}^2\hfill \\ {\mathbf{\Gamma }}_{k-1}=T\left(\mathit{I}+\frac{1}{2!}{\mathit{F}}_{k-1}T+\frac{1}{3!}{\mathit{F}}_{k-1}^2{T}^2\right){\mathit{G}}_k\hfill \end{array}$$

(39)

The Kalman filter algorithm is expressed as follows:

The one-step prediction equation:

$${\widehat{\mathit{X}}}_{k+1|k}={\mathbf{\Phi }}_k{\mathit{X}}_k$$

(40)

The state estimation:

$${\widehat{\mathit{X}}}_{k+1}={\widehat{\mathit{X}}}_{k+1\left|k\right.}+{\mathit{K}}_k\left({\mathit{Z}}_{k+1}-\mathit{H}{\widehat{\mathit{X}}}_{k+1|k}\right)$$

(41)

The gain of the filter:

$${\mathit{K}}_k={\mathit{P}}_{k+1|k}{\mathit{H}}^{\mathrm{T}}{\left(\mathit{H}{\mathit{P}}_{k+1|k}{\mathit{H}}^{\mathrm{T}}+{\mathit{R}}_k\right)}^{-1}$$

(42)

The MSE of the one-step prediction:

$${\mathit{P}}_{k+1|k}={\mathbf{\Phi }}_k{\mathit{P}}_k{\mathbf{\Phi }}_k^{\mathrm{T}}+{\mathbf{\Gamma }}_k{\mathit{Q}}_{k+1}{\mathbf{\Gamma }}_k^{\mathrm{T}}$$

(43)

The MSE of the estimation:

$${\mathit{P}}_{k+1}=\left(\mathit{I}-{\mathit{K}}_{k+1}\mathit{H}\right){\mathit{P}}_{k+1|k}$$

(44)

The above are the basic equations of the discrete Kalman filter. When the initial value ${\widehat{\mathit{X}}}_0$ and ${\mathit{P}}_0$ are given, combined with the measurement ${\mathit{Z}}_k$ of the k moment, the state estimation ${\widehat{\mathit{X}}}_k$ at k moment can be obtained through iteration.

5. Simulation and Analysis

The Shandong Peninsula, the Liaodong Peninsula and the Yangtze River Delta were selected as the navigation landmarks. The landmark cameras were used to photograph the landmarks in the flight segment of the aircraft, and then the landmarks were compared to obtain the position and attitude of the aircraft. The matching process is shown in Figure 5, Figure 6 and Figure 7.

The trajectory of the spacecraft was drawn based on the captured information, as shown in Figure 8, Figure 9 and Figure 10.

The spacecraft was analyzed in the simulation, and the error parameters of SINS and landmark navigation were set as follows.

① Initial position errors of the three directions wer all 0 m. ② The initial speed errors were 0 m/s. ③ The initial attitude angle error was 0${}^{″}$. ④ The gyro random constant drift was ${0.1}^{\circ }/h$. ⑤ The gyroscope random noise drift satisfied a normal distribution with a mean of 0 and a standard deviation of 0.5${}^{″}$. ⑥ The accelerometer random constant bias was ${10}^{-4}g$, where g is the Earth gravity constant. ⑦ The accelerometer random noise offset satisfied a normal distribution with a mean of 0 and a standard deviation of $5\times {10}^{-5}g$. ⑧ The standard deviation of the landmark navigation output position error was 50 m. ⑨ The standard deviation of the landmark navigation output attitude error was 3${}^{″}$.

The sampling interval of SINS was 0.01 s, the landmark navigation was 0.1 s, the combined interval was 0.1 s, the simulation time was 1110 s, and the Monte Carlo method was used for simulation. Due to the requirements of the launch inertial system, initial latitude and longitude were ${116.34}^{\circ }$ and ${39.98}^{\circ }$, respectively, and the height was 0 m. The platform angle error, gyro drift, position error and speed error were corrected by the feedback during the navigation process.

To verify the importance of the landmark attitude determination in the integrated navigation, two methods (with attitude determine and without attitude determine) were simulated, and the trajectories of the aircraft was obtained, as shown in Figure 11.

It can be seen in Figure 11 that, during the whole navigation process, the orbit determination of the aircraft with attitude determination is almost the same as the real orbit, while the trajectory without attitude determination is far from the real orbit, which indicates that the attitude determination is critical. The comparison of the position as well as the velocity determination in three directions are given, respectively.

In Figure 12 , we can find that, in the X direction, when there is no attitude determination, the error of the position gradually becomes larger before 900 s, and there is a convergence trend after 900 s: the velocity error becomes larger with time. With the attitude determination, the position and velocity errors can be kept near 0. In Figure 13, for Y direction, without the attitude determination, the position and velocity errors diverge over time. With the attitude determination, the position error is very small, almost at 0 level and the speed figure has a slight error, but it also has a better improvement. In Figure 14, for Z direction, without the attitude determination, the position error diverges with time, the velocity error is divergent before 160 s, and then gradually converges after 160 s. When with the attitude determination, the changes of the position and velocity errors are similar to the Y direction.

Through the comparison of the three directions, it can be seen that the position errors gradually increase with time when the landmark only provides the positioning information, and large offsets appear in three directions, leading the gradually decreasing of the navigation accuracy. When the landmark completes the attitude determination, the navigation result can remain stable for a long time. The velocity determination is similar to the position determination.

Figure 12, Figure 13 and Figure 14 express the qualitative analysis of the accuracy on whether determine the attitude in the three directions, while

Table 2. Position accuracy data statistics (unit: m).

		X	Y	Z
With attitude determination	The average error	25.6573	60.1919	175.0093
	The maximum error	40.5812	116.1984	240.7109
Without attitude determination	The average error	1004.2	2606.1	7664.0
	The maximum error	1601.2	6251.0	13615

and

Table 3. Velocity accuracy data statistics (unit: m/s).

		${\mathit{V}}_{\mathit{X}}$	${\mathit{V}}_{\mathit{Y}}$	${\mathit{V}}_{\mathit{Z}}$
With attitude determination	The average error	0.5100	1.1333	3.4702
	The maximum error	1.2567	2.0557	6.2684
Without attitude determination	The average error	1.6633	5.5829	12.1824
	The maximum error	3.1103	7.2776	19.0498

express the quantitative accuracy analysis of it.

It can be seen in Table 2 and Table 3 that it is quite different whether there is the attitude determination for navigation, and the attitude determination has a large impact on the navigation accuracy. When the attitude determination process is completed, the position and the velocity of the integrated navigation maintain at a high precision, and the navigation result approximates the true.

6. Conclusions

In this paper, faced with the limitation of the traditional SINS/CNS integrated navigation that the system is difficult to navigate all day and all weather, a SINS/Landmark integrated navigation method based on landmark attitude determination is proposed. Through the theoretical analysis of the landmark navigation principle and the simulation of the SINS/Landmark integrated navigation system, the landmark information is necessary to determine the attitude. When the landmark navigation only extracts the position information, the accuracy of the integrated navigation will maintain a large range of fluctuations. After the attitude determination, the accuracy of the integrated navigation can reach a high level and does not decrease with time. This shows that the SINS/Landmark integrated navigation method based on the landmark attitude determination is indeed effective, which can improve the accuracy of the navigation to some extent.