# Reliable SINS/SAR/GNSS Integrated Navigation System for the Supersonic Vehicle

Authors: Pengxiang Yang; Qiju Zhu; Chunbo Mei; Zhenhui Fan; Baofeng Lu; Honglong Jin
Journal: Journal of Sensors
 (2022)

---

## Abstract

In the GNSS-challenged/denied environments, compared with other aided navigation systems, synthetic aperture radar- (SAR-) aided navigation systems can achieve all-weather, all-day, and global positioning. This paper is aimed at designing a reliable navigation system for the supersonic vehicle in the whole flight phase. Considering the SAR is of great significance for the reliable and precise navigation of the supersonic vehicle, we introduce it to the terminal phase to cope with the interference of the global navigation satellite system (GNSS). At the same time, to accelerate the speed of SAR positioning, a positioning method based on the area feature and stack sequential decoding algorithm (SSDA) is designed. Then, applying the SAR positioning under the framework of the Kalman filter (KF), we propose a tightly coupled strapdown inertial navigation system (SINS)/SAR/GNSS integration algorithm, which can achieve positioning without a complicated fusion method between the front phase and terminal phase. The flight experiment results show that the proposed method can correct the average position error to 1.7 m within 1 cycle.


---

## Body

## 1. Introduction

At present, medium-range supersonic vehicle rely more on global navigation satellite system (GNSS) and inertial navigation system (INS) [1]. However, the significant problem with using GNSS technology is that the signals are very weak [2]. Taking GNSS as an example, the signals are transmitted at two L-band frequencies: L1=1:57542GHz and L2=1:2276GHz from a set of orbiting satellites located at an altitude of ~20,200 Km [3]. Since the global positioning system (GPS) signals are transmitted at only ~25 W, the received signal at a typical GPS receiver is on the order of -155 to 160 dBW, which makes GNSS signals extremely susceptible to interference [4–6]. Therefore, in this case, how to design the navigation system for medium-range supersonic vehicle is vital.To tackle the positioning problem in the GPS-denied environment, Poulose et al. proposed a novel fusion algorithm based on the traditional inertial measurement unit (IMU) and magnetometer, which firstly completes the sensor fusion algorithm for pitch and roll estimation and then completes the sensor fusion algorithm for heading estimation. Furthermore, based on an accuracy generalization benchmark for indoor localization [7], experimental results show that the proposed position-estimation algorithm achieves high position accuracy that significantly outperforms that of conventional estimation methods [8]. Gao et al. proposed a SINS/SAR loosely integrated method based on the Lupin adaptive filter, which establishes the SINS state equation as a linear model and uses SAR latitude, longitude, and height as the observation to establish the observation equation. Compared with the traditional adaptive Kalman filter, it can suppress the state divergence caused by the state noise and the observation noise [9]. Xiao et al. designed a switching mechanism between SINS/GPS and SINS/SAR to reduce the impact of matching errors on the combined system during the SAR image matching process [10]. In addition to using the latitude and longitude provided by SAR as the observation data, Xu et al. proposed an SINS/SAR loosely integrated method, which makes full use of the heading angle provided by the SAR scene matching and the east and north velocity as observations. It improves the integrated accuracy of SINS/SAR navigation information [11]. Jiang et al. proposed a SINS/SAR loosely integrated method, which uses the interferogram obtained by interferometric synthetic aperture radar (InSAR) to obtain three-dimensional positioning latitude and longitude. Compared with SAR, InSAR can obtain higher precision elevation and provide high-precision elevation correction for SINS [12]. Yandong et al. based on the SINS linear error model and SAR measurement of latitude and longitude designed an adaptive Kalman filter method to complete the SINS/SAR combination [13]. Similarly, Gao et al. designed an error suppression adaptive Kalman filter to complete the SINS/SAR combination [14]. In the latter, they proposed a SINS/GPS/SAR tightly integrated method, which uses SAR azimuth, elevation, GPS pseudorange, and pseudorange rate to establish a measurement model. As the author mentioned, the advantage of this method is that the GPS and SAR can calibrate each other under extreme conditions. However, when the GPS interferes, it is difficult to ensure the high accuracy of carrier positioning only using SAR azimuth and elevation angles [15]. Lu et al. designed an SINS/SAR tightly integrated method, which focused on the impact of the single-sided and double-sided distribution of control points on the INS/SAR combined accuracy [16].Different from the existing algorithm, this paper proposes a tightly coupled SINS/SAR/GNSS integration algorithm for the navigation of the supersonic vehicle in the whole flight phase. The main contributions of this paper are as follows:(i)
A SINS/SAR/GNSS tightly integrated navigation system architecture is proposed based on KF, which is equipped with a partial feedback update algorithm to tackle the high requirements for the convergence speed and equipped with an approximate extrapolation to solve nonequidistance and delayed measurement of SAR(ii)
Aiming at the problem of SAR delayed measurement, a SINS/SAR/GNSS measurement update algorithm is proposed, which can be performed in real-time in FPGAThe rest of this paper is organized as follows: in Section2, we introduce the tightly coupled SINS/SAR/GNSS model based on the Kalman filter, which equipped with a partial feedback update algorithm to tackle the high requirements for the convergence speed and equipped with an approximate extrapolation to solve nonequidistance and delayed measurement of SAR. In Section 3, we conduct a flight experiment to verify the effectiveness of the proposed algorithm, and the position error is discussed. In Section 4, we discuss the experiment results, the proposed method, and the future work. Finally, conclusions are given in Section 5.


## 2. Methods

In short, the mainstream of the SINS/SAR can be divided into three parts according to the fusion algorithm. The first is taking the system model as nonlinear and then designing SINS/SAR integrated methods based on PF and its improved methods. It can be predicted that these methods are time-consuming. The second is taking the system model as linear and then designing SINS/SAR loosely integrated methods based on KF and its improved method, but only using the SAR observation longitude, latitude, height, and heading angle as measurements, which was greatly affected by the SAR matching accuracy. The third is taking the system model as linear and then designing SINS/SAR integrated methods based on KF and its improved methods, but considering the nonequidistance and delayed measurement of the actual SAR sensor, these methods did not give corresponding solutions.Different from the existing algorithm, this paper proposes a tightly coupled SINS/SAR/GNSS integration algorithm for the navigation of the supersonic vehicle in the whole flight phase, as described in Section2.1. The SAR positioning algorithm is described in Section 2.2. The system model and fusion algorithm are described in Section 2.3. Aiming at the problem of SAR delayed measurement, a SINS/SAR/GNSS measurement update algorithm is proposed, as described in Section 2.4.


### 2.1. System Architecture

As shown in Figure1, the typical flight phase of a medium-range supersonic vehicle consists of three stages [17]. In the boost phase and the midcourse phase, the GNSS signal is valid, and the tightly coupled SINS/GNSS is used as the navigation method. When the supersonic vehicle enters into the terminal phase, the GNSS would be interfered, so the SAR-aided navigation system is introduced. Considering the relative position of the vehicles and jamming radar in the terminal phase, the satellite signals may not be completely jammed [18]; thus, a tightly coupled SINS/SAR/GNSS algorithm is proposed in the terminal phase.Figure 1
The typical flight phase of a supersonic vehicle.In the boost phase and midcourse phase, since the GNSS is not interfered, the tightly SINS/GNSS integrated navigation system is used for navigation. Meanwhile, the high power consumption of SAR only allows limited use, so SAR will not be used in the two-phase. Then, in the terminal phase, when the GNSS is interfered, the tightly SINS/SAR/GNSS integrated navigation system is performed. And if the GNSS is interfered completely, the system degenerates to SINS/SAR.As illustrated in Figure2, the proposed system can be divided into four parts: SINS mechanization, system model, integration Kalman filter, and measurement processing. Considering the real-time performance of the vehicle-borne solution, we adopt the quaternion attitude update algorithm described in [19, 20] and the velocity and position update algorithm proposed in [21–23].Figure 2
The system architecture of the tightly coupled integrated navigation system of SINS/SAR/GNSS.In general, the update period of SAR measurement is greater than that of GNSS and greater than that of IMU. When the single-sample algorithm is used, the update period of SINS is the same as that of IMU. Considering that the SAR measurement sequence will inevitably be nonequidistance and delayed because of the high time consuming when processing the SAR echo, which can be seen in Figure3, an approximate extrapolation algorithm is proposed under the framework of KF. And the measurement processing is introduced in Section 2.4.Figure 3
The time sequence of the tightly coupled SINS/SAR/GNSS.


### 2.2. SAR Positioning by Template Match

Although the measurement delay of SAR is inevitable, the shorter the matching time, the higher the accuracy of the integrated navigation system. At present, image matching is commonly conducted by feature extracting and matching, such as the ORB, SAR-SIFT, and LSD. However, all of these methods are extremely time-consuming. To tackle this, we proposed an area feature extraction and match algorithm based on the area feature and SSDA. Since SAR imaging is largely determined by the reflectivity of the echo from the ground, we utilize the pixel value to extract the scatter feature and then utilize the region growing method to generate the area feature, which is described in Figures4(a) and 4(b). Considering that before the launch of the vehicle, the maximum position errors could be inferred based on the sensor error. Thus, the reference map is made by 1.5 times the range direction and 2.5 times the azimuth direction, respectively, of the SAR, which is described in Figure 4(c). After the area features of the real-time SAR image are extracted, we perform the SSDA (sequential similarity detection algorithm) to match it (P‐uv) with the reference map (O‐xy), which is described in Figure 4(e).Figure 4
SAR positioning by template matching of area feature. (a) The SAR image. (b) The area feature of SAR image. (c) The reference map. (d) The color reference map. (e) The result of template matching of SAR image and reference map.We conduct a comparative experiment on the mainstream feature algorithm of LSD [24], SAR-SIFT [25], ORB [26], and our method with the computer of Intel Core i5-4690K. The experiment results are shown in Table 1. Compared with other methods, our method requires fewer calculations and can achieve higher accuracy.Table 1
The comparative experiment results of feature algorithms.
Feature algorithmMatching algorithmSAR image sizeAverage position error (pixel)Average time of the whole process (s)ORBFLANN [27]650∗18001.70.11SAR-SIFTFLANN [27]650∗18001.60.14LSDLSD650∗18002.40.19OursSSDA650∗18001.90.09


### 2.3. System Model

With the navigation frame defined in the east-north-up (E-N-U) geography frame, the SINS/SAR integrated system model can be written as:(1)Ẋt=FtXt+r,Żt=HtXt+q,where Xt is the error-state vector of SINS which has 17 dimensions, Ft is the state transition matrix which is 17∗17 dimensions, r is the vector of process noise which has 17 dimensions, q is the vector of measurement noise which has 17 dimensions, Zt is the vector of measurement which has 6 dimensions, and Ht is the observation matrix constructed based on pseudorange and pseudorange rate which is 6∗17 dimensions.As expressed in (2), the state vector X has 17 dimensions, which is composed by 3-dim attitude errors, 3-dim velocity errors, 3-dim position errors, 3-dim gyroscope drifts, 3-dim accelerometer drifts, 1-dim pseudorange error, and 1-dim pseudorange rate error:
(2)X=δVEn,δVNn,δVUn,ϕEn,ϕNn,ϕUn,δL,δλ,δh,εx,εy,εz,∇x,∇y,∇z,δrs,δvs.Based on the state vector, the state transition matrixF can be written as:
(3)F=FvϕFvvFvp−Cnb03∗303∗2FϕvFϕϕFϕp03∗3Cnb03∗20pvFpϕFpp03∗303∗303∗206∗306∗306∗306∗306∗303∗2000001000000.For the convenience of algorithmic calculation, the pseudorange and pseudorange rate in measurement equation are calculated in the ECEF (Earth-Centered Earth-Fixed) frame. We firstly transform them to the ECEF frame:(4)xIyIzI=RN+hIcosLIcosλIRN+hIcosLIsinλIRN1−e2+hIsinLI,xSySzS=RN+hScosLScosλSRN+hScosLSsinλSRN1−e2+hSsinLS,where LI,λI,hI is the position of the SINS mechanization and xI,yI,zI is the position of SINS in the ECEF frame. Similarly, LS,λS,hS and xS,yS,zS denote the position of measurement sensors (control points of SAR of satellite).Then, a first-order approximation at the true value of positionx,y,z is carried out to calculate the pseudorange ρI and pseudorange rate ρ̇I, and we can get the following equations after dropping higher-order error terms:
(5)ρI=ρ+∂ρI∂xδx+∂ρI∂yδy+∂ρI∂zδz=ρ+x−xSρδx+y−ySρδy+z−zSρδz,ρ̇I=ρ̇+xρ−x−xsρ̇ρ2δx+yρ−y−ysρ̇ρ2δy+zρ−z−zsρ̇ρ2δz+x−xsρδẋ+y−ysρδẏ+z−zsρδż.Thus, according to the measurement pseudorange and pseudorange rate, the measurement model in Equation (1) can be calculated as:
(6)Zt=ZrtZυt=ρI−ρGρ̇I−ρ̇G=Ht⋅Xt+q1q2,where the Zrt is the pseudorange measurement model and the Zvt is the pseudorange rate measurement model. And the Tne is the transformation of velocity from the navigation frame system to the ECEF frame:
(7)TneLI,λI=−sinLI−sinLIcosλIcosLIcosλIcosλI−sinLIsinλIcosLIsinλI0cosLIsinLI.In Equation (6), the observation matrix is given as:
(8)Ht=01∗301∗3PTeECEF01∗9−1001∗3P⋅TneQ⋅TeECEF+P⋅δTne01∗90−1,where the P is the retraction of:
(9)P=xI−xG/ρyI−yGρyI−yG/ρ.TheQ is the retraction of:
(10)Q=xρ−x−xsρ̇ρ2yρ−y−ysρ̇ρ2zρ−z−zsρ̇ρ2.TheδTne is the retraction of:
(11)δTne=−VNnsinLcosλ−VUnsinLcosλ−VEncosλ+VNnsinLsinλ0−VNncosLsinλ−VUnsinLsinλ−VEnsinλ−VNnsinLcosλ0−VNnsinL−VUncosL00.TheTeECEF is the retraction of the transformation between δL,δλ,δh and δx,δy,δz:
(12)TeECEF=−RN+hsinLcosλ−RN+hcosLsinλcosLcosλ−RN+hsinLsinλRN+hcosLcosλcosLsinλRN1−e2+hcosL0sinL.


### 2.4. Kalman Filter with Feedback and Measurement Delay

The time update step of Kalman filter with feedback can be written as:(13)Xk/k−1=FkXk−1,Pk/k−1=FkPk−1FkT+Qk,where for the convenience of algorithmic calculation, the discrete form of error-state is expressed as: Fk=I17∗17+FtT. The Pk is the state covariance matrix at k frame. The Qk is the process noise covariance matrix which is usually a constant value. Further, the measurement update step of Kalman filter with feedback can be written as:
(14)Kk=Pk/k−1HkTHkPkHkT+RkT,Xk=Xk/k−1+KkZk−HkPk/k−1,Pk=I−KkHkPk/k−1,where the Kk is the gain matrix of the Kalman filter. When multiple pseudorange and pseudorange rate data are received, whether they are from GNSS or SAR, performing multiple iterations according to (12), followed with Kalman filter, the mechanization results of the velocity Vkn, attitude matrix Cn/kb, and position λk,Lk,hk at k frame are corrected by:
(15)Vkn=Vk−1n−Xk1:3,Cn/kb=I+Xk4:6×Cn/k−1b,λk,Lk,hkT=λk−1,Lk−1,hk−1T−Xk7:9.After the process of correction, we set the attitude error, velocity error, and the pseudorange error to zero as the feedback. Considering the measurement delay of SAR, we utilize the approximate extrapolation algorithm to calculate the stateϕkn,δVkn,δPkn at k frame.Firstly, the stateϕk−dln,δVk−dln,δPk−dln that is at the actual measurement frame of SAR is calculated by:
(16)ϕk−dln=ϕk/k−1n−−ωinn×ϕk/k−1nTnavNdl,δVk−dln=δVk/k−1−finn×ϕk/k−1nTnavNdl,δPk−dln=δPk/k−1n,where the delay period Ndl can be calculated based on the delay time returned by the SAR measurement. The Tnav is the mechanization period of navigation. The ϕk/k−1n,δVk/k−1n,δPk/k−1n is calculated and saved according to Equation (13).Then, the measurement update at current time can be calculated as:(17)ϕkn=ϕk−dln+−ωinn×ϕk/k−dlnTnavNdl,δVkn=δVk−dln+−finn×ϕk/k−dlnTnavNdl,δPkn=δPk−dln+δVk/k−dlnTnavNdl.Note that when the SAR measurement comes, the chi-square test is used to judge the validity of the measurement. Firstly, we calculate the innovation variance matrix byPI=HkPk/k−1HkT+Rk based on the saved historical data. Then, we calculate the innovation matrix by I=Zk−HkXk/k−1. The measurement is valid only when the data in the vector I is less than 3 times the diagonal data in the matrix PI.


## 2.1. System Architecture

As shown in Figure1, the typical flight phase of a medium-range supersonic vehicle consists of three stages [17]. In the boost phase and the midcourse phase, the GNSS signal is valid, and the tightly coupled SINS/GNSS is used as the navigation method. When the supersonic vehicle enters into the terminal phase, the GNSS would be interfered, so the SAR-aided navigation system is introduced. Considering the relative position of the vehicles and jamming radar in the terminal phase, the satellite signals may not be completely jammed [18]; thus, a tightly coupled SINS/SAR/GNSS algorithm is proposed in the terminal phase.Figure 1
The typical flight phase of a supersonic vehicle.In the boost phase and midcourse phase, since the GNSS is not interfered, the tightly SINS/GNSS integrated navigation system is used for navigation. Meanwhile, the high power consumption of SAR only allows limited use, so SAR will not be used in the two-phase. Then, in the terminal phase, when the GNSS is interfered, the tightly SINS/SAR/GNSS integrated navigation system is performed. And if the GNSS is interfered completely, the system degenerates to SINS/SAR.As illustrated in Figure2, the proposed system can be divided into four parts: SINS mechanization, system model, integration Kalman filter, and measurement processing. Considering the real-time performance of the vehicle-borne solution, we adopt the quaternion attitude update algorithm described in [19, 20] and the velocity and position update algorithm proposed in [21–23].Figure 2
The system architecture of the tightly coupled integrated navigation system of SINS/SAR/GNSS.In general, the update period of SAR measurement is greater than that of GNSS and greater than that of IMU. When the single-sample algorithm is used, the update period of SINS is the same as that of IMU. Considering that the SAR measurement sequence will inevitably be nonequidistance and delayed because of the high time consuming when processing the SAR echo, which can be seen in Figure3, an approximate extrapolation algorithm is proposed under the framework of KF. And the measurement processing is introduced in Section 2.4.Figure 3
The time sequence of the tightly coupled SINS/SAR/GNSS.


## 2.2. SAR Positioning by Template Match

Although the measurement delay of SAR is inevitable, the shorter the matching time, the higher the accuracy of the integrated navigation system. At present, image matching is commonly conducted by feature extracting and matching, such as the ORB, SAR-SIFT, and LSD. However, all of these methods are extremely time-consuming. To tackle this, we proposed an area feature extraction and match algorithm based on the area feature and SSDA. Since SAR imaging is largely determined by the reflectivity of the echo from the ground, we utilize the pixel value to extract the scatter feature and then utilize the region growing method to generate the area feature, which is described in Figures4(a) and 4(b). Considering that before the launch of the vehicle, the maximum position errors could be inferred based on the sensor error. Thus, the reference map is made by 1.5 times the range direction and 2.5 times the azimuth direction, respectively, of the SAR, which is described in Figure 4(c). After the area features of the real-time SAR image are extracted, we perform the SSDA (sequential similarity detection algorithm) to match it (P‐uv) with the reference map (O‐xy), which is described in Figure 4(e).Figure 4
SAR positioning by template matching of area feature. (a) The SAR image. (b) The area feature of SAR image. (c) The reference map. (d) The color reference map. (e) The result of template matching of SAR image and reference map.We conduct a comparative experiment on the mainstream feature algorithm of LSD [24], SAR-SIFT [25], ORB [26], and our method with the computer of Intel Core i5-4690K. The experiment results are shown in Table 1. Compared with other methods, our method requires fewer calculations and can achieve higher accuracy.Table 1
The comparative experiment results of feature algorithms.
Feature algorithmMatching algorithmSAR image sizeAverage position error (pixel)Average time of the whole process (s)ORBFLANN [27]650∗18001.70.11SAR-SIFTFLANN [27]650∗18001.60.14LSDLSD650∗18002.40.19OursSSDA650∗18001.90.09


## 2.3. System Model

With the navigation frame defined in the east-north-up (E-N-U) geography frame, the SINS/SAR integrated system model can be written as:(1)Ẋt=FtXt+r,Żt=HtXt+q,where Xt is the error-state vector of SINS which has 17 dimensions, Ft is the state transition matrix which is 17∗17 dimensions, r is the vector of process noise which has 17 dimensions, q is the vector of measurement noise which has 17 dimensions, Zt is the vector of measurement which has 6 dimensions, and Ht is the observation matrix constructed based on pseudorange and pseudorange rate which is 6∗17 dimensions.As expressed in (2), the state vector X has 17 dimensions, which is composed by 3-dim attitude errors, 3-dim velocity errors, 3-dim position errors, 3-dim gyroscope drifts, 3-dim accelerometer drifts, 1-dim pseudorange error, and 1-dim pseudorange rate error:
(2)X=δVEn,δVNn,δVUn,ϕEn,ϕNn,ϕUn,δL,δλ,δh,εx,εy,εz,∇x,∇y,∇z,δrs,δvs.Based on the state vector, the state transition matrixF can be written as:
(3)F=FvϕFvvFvp−Cnb03∗303∗2FϕvFϕϕFϕp03∗3Cnb03∗20pvFpϕFpp03∗303∗303∗206∗306∗306∗306∗306∗303∗2000001000000.For the convenience of algorithmic calculation, the pseudorange and pseudorange rate in measurement equation are calculated in the ECEF (Earth-Centered Earth-Fixed) frame. We firstly transform them to the ECEF frame:(4)xIyIzI=RN+hIcosLIcosλIRN+hIcosLIsinλIRN1−e2+hIsinLI,xSySzS=RN+hScosLScosλSRN+hScosLSsinλSRN1−e2+hSsinLS,where LI,λI,hI is the position of the SINS mechanization and xI,yI,zI is the position of SINS in the ECEF frame. Similarly, LS,λS,hS and xS,yS,zS denote the position of measurement sensors (control points of SAR of satellite).Then, a first-order approximation at the true value of positionx,y,z is carried out to calculate the pseudorange ρI and pseudorange rate ρ̇I, and we can get the following equations after dropping higher-order error terms:
(5)ρI=ρ+∂ρI∂xδx+∂ρI∂yδy+∂ρI∂zδz=ρ+x−xSρδx+y−ySρδy+z−zSρδz,ρ̇I=ρ̇+xρ−x−xsρ̇ρ2δx+yρ−y−ysρ̇ρ2δy+zρ−z−zsρ̇ρ2δz+x−xsρδẋ+y−ysρδẏ+z−zsρδż.Thus, according to the measurement pseudorange and pseudorange rate, the measurement model in Equation (1) can be calculated as:
(6)Zt=ZrtZυt=ρI−ρGρ̇I−ρ̇G=Ht⋅Xt+q1q2,where the Zrt is the pseudorange measurement model and the Zvt is the pseudorange rate measurement model. And the Tne is the transformation of velocity from the navigation frame system to the ECEF frame:
(7)TneLI,λI=−sinLI−sinLIcosλIcosLIcosλIcosλI−sinLIsinλIcosLIsinλI0cosLIsinLI.In Equation (6), the observation matrix is given as:
(8)Ht=01∗301∗3PTeECEF01∗9−1001∗3P⋅TneQ⋅TeECEF+P⋅δTne01∗90−1,where the P is the retraction of:
(9)P=xI−xG/ρyI−yGρyI−yG/ρ.TheQ is the retraction of:
(10)Q=xρ−x−xsρ̇ρ2yρ−y−ysρ̇ρ2zρ−z−zsρ̇ρ2.TheδTne is the retraction of:
(11)δTne=−VNnsinLcosλ−VUnsinLcosλ−VEncosλ+VNnsinLsinλ0−VNncosLsinλ−VUnsinLsinλ−VEnsinλ−VNnsinLcosλ0−VNnsinL−VUncosL00.TheTeECEF is the retraction of the transformation between δL,δλ,δh and δx,δy,δz:
(12)TeECEF=−RN+hsinLcosλ−RN+hcosLsinλcosLcosλ−RN+hsinLsinλRN+hcosLcosλcosLsinλRN1−e2+hcosL0sinL.


## 2.4. Kalman Filter with Feedback and Measurement Delay

The time update step of Kalman filter with feedback can be written as:(13)Xk/k−1=FkXk−1,Pk/k−1=FkPk−1FkT+Qk,where for the convenience of algorithmic calculation, the discrete form of error-state is expressed as: Fk=I17∗17+FtT. The Pk is the state covariance matrix at k frame. The Qk is the process noise covariance matrix which is usually a constant value. Further, the measurement update step of Kalman filter with feedback can be written as:
(14)Kk=Pk/k−1HkTHkPkHkT+RkT,Xk=Xk/k−1+KkZk−HkPk/k−1,Pk=I−KkHkPk/k−1,where the Kk is the gain matrix of the Kalman filter. When multiple pseudorange and pseudorange rate data are received, whether they are from GNSS or SAR, performing multiple iterations according to (12), followed with Kalman filter, the mechanization results of the velocity Vkn, attitude matrix Cn/kb, and position λk,Lk,hk at k frame are corrected by:
(15)Vkn=Vk−1n−Xk1:3,Cn/kb=I+Xk4:6×Cn/k−1b,λk,Lk,hkT=λk−1,Lk−1,hk−1T−Xk7:9.After the process of correction, we set the attitude error, velocity error, and the pseudorange error to zero as the feedback. Considering the measurement delay of SAR, we utilize the approximate extrapolation algorithm to calculate the stateϕkn,δVkn,δPkn at k frame.Firstly, the stateϕk−dln,δVk−dln,δPk−dln that is at the actual measurement frame of SAR is calculated by:
(16)ϕk−dln=ϕk/k−1n−−ωinn×ϕk/k−1nTnavNdl,δVk−dln=δVk/k−1−finn×ϕk/k−1nTnavNdl,δPk−dln=δPk/k−1n,where the delay period Ndl can be calculated based on the delay time returned by the SAR measurement. The Tnav is the mechanization period of navigation. The ϕk/k−1n,δVk/k−1n,δPk/k−1n is calculated and saved according to Equation (13).Then, the measurement update at current time can be calculated as:(17)ϕkn=ϕk−dln+−ωinn×ϕk/k−dlnTnavNdl,δVkn=δVk−dln+−finn×ϕk/k−dlnTnavNdl,δPkn=δPk−dln+δVk/k−dlnTnavNdl.Note that when the SAR measurement comes, the chi-square test is used to judge the validity of the measurement. Firstly, we calculate the innovation variance matrix byPI=HkPk/k−1HkT+Rk based on the saved historical data. Then, we calculate the innovation matrix by I=Zk−HkXk/k−1. The measurement is valid only when the data in the vector I is less than 3 times the diagonal data in the matrix PI.


## 3. Experiment and Result Discussion

To testify the effectiveness of the proposed algorithm, a flight experiment is carried out. The velocity of the vehicle in the whole flight phase is within 900-1100 m/s, and the flight time is about 160 s in the terminal phase of 120-200 Km. The experiment setup in this experiment is described in Section3.1. The parameters of the algorithm are given in Section 3.2. The experiment results are described in Section 3.3.


### 3.1. Experiment Setup

The parameters of SINS, SAR, and GNSS receiver are given in Tables2, 3, and 4, respectively. Due to the limited experimental conditions, we do not conduct GNSS jamming tests. But after analyzing the experimental data, we divided the flight phase based on the flight mileage. In the terminal phase, we select satellite data from 0 to 1 randomly as the GNSS data for the tightly coupled integrated navigation system, which can be regarded as the case of GNSS interference.Table 2
Parameters of SINS.
ParametersUnitValueAccelerometer biasμg1001σAccelerometer scale factorppm1001σAccelerometer installationarcsec201σAccelerometer white noiseμg/Hz301σGyroscope bias°/h0.11σGyroscope scale factorppm1001σGyroscope installationarcsec201σGyroscope white noise°/h/Hz0.031σUpdate cyclems0.002Mechanization cyclems0.004Table 3
Parameters of SAR.
ParametersUnitValueUpdate cycles2Pseudorange errorm21σPseudorange rate errorm/s0.11σTable 4
Parameters of GNSS receiver.
ParametersUnitValueUpdate cyclems100Clock correction cyclems100Pseudorange errorm6.5Pseudorange rate errorm/s0.9Due to the high power consumption of SAR, we perform four imaging in the terminal phase, which provides four measurement information and is described in Figure5.Figure 5
The SAR positioning in the terminal phase. (a) The trajectory of vehicle flight. (b) The SAR image.
(a)(b)


### 3.2. Experiment Parameters

The setting of the initial value of the state covariance matrixP, the value of the process noise covariance matrix Q, and the value of the measurement noise covariance matrix R play an important role in the convergence and convergence speed of the filter.As for the 17-dimensional diagonal matrixP, the covariance of the velocity and position is set according to the initial error. The covariance of the attitude is set according to the statistical variance of the alignment result. The covariance of gyro and the accumulator bias are set according to their repeatability. The covariance of pseudorange error and the pseudorange rate error is set according to the clock error parameters and the clock rate error parameters of the measurement. Based on the rules above, in the flight experiment the initial value of the P is set to:
(18)P0=diag0.02;2;0.02∗π180;5;5;5;10006378137;10006378137;1000;0.13600;0.13600;0.13600∗π180;0.5;0.5;0.5∗9.8106;0.25;0.0025.Furthermore, bothQ and R are set to constant values. The component noise covariance corresponding to the attitude and velocity in the Q is set to the noise of the gyro and accelerometer separately, and the rest are small values. The component noise covariance corresponding to the pseudorange of the R matrix is set to 1, and the component corresponding to the pseudorange rate is set to 0.1.


### 3.3. Results

The experiment results of longitude error, latitude error, and altitude error are given in Figures6, 7, and 8, respectively. At 42 s, the GNSS signals began to be jammed. Thus, in the time range of [0, 41.9] s and [42.0, 50.0] s, we perform the tightly coupled SINS/GNSS and tightly coupled SINS/SAR/GNSS integration algorithm, respectively.Figure 6
Results of longitude error. (a) The longitude error in the whole phase. (b) The longitude error in the terminal phase.
(a)(b)Figure 7
Results of latitude error. (a) The latitude error in the whole phase. (b) The latitude error in the terminal phase.
(a)(b)Figure 8
Results of altitude error. (a) The altitude error in the whole phase. (b) The altitude error in the terminal phase.
(a)(b)As shown above, when the GNSS signal is interfered, the SINS/GNSS will cause an average position error of 2.8 m within 2 seconds, which is intolerable for supersonic vehicles in the terminal phase. At the moment of 43.9 s, 45.9 s, 47.9 s, and 49.9 s, the SINS/SAR/GNSS algorithm is utilized, and the average position error could be corrected to 1.7 m within 1 cycle. With the relatively few measurement times and the long measurement cycle, the measurement of SAR can suppress the divergence of SINS/GNSS positioning, and the proposed algorithm can achieve high positioning accuracy, which meets the requirements of supersonic vehicles.


## 3.1. Experiment Setup

The parameters of SINS, SAR, and GNSS receiver are given in Tables2, 3, and 4, respectively. Due to the limited experimental conditions, we do not conduct GNSS jamming tests. But after analyzing the experimental data, we divided the flight phase based on the flight mileage. In the terminal phase, we select satellite data from 0 to 1 randomly as the GNSS data for the tightly coupled integrated navigation system, which can be regarded as the case of GNSS interference.Table 2
Parameters of SINS.
ParametersUnitValueAccelerometer biasμg1001σAccelerometer scale factorppm1001σAccelerometer installationarcsec201σAccelerometer white noiseμg/Hz301σGyroscope bias°/h0.11σGyroscope scale factorppm1001σGyroscope installationarcsec201σGyroscope white noise°/h/Hz0.031σUpdate cyclems0.002Mechanization cyclems0.004Table 3
Parameters of SAR.
ParametersUnitValueUpdate cycles2Pseudorange errorm21σPseudorange rate errorm/s0.11σTable 4
Parameters of GNSS receiver.
ParametersUnitValueUpdate cyclems100Clock correction cyclems100Pseudorange errorm6.5Pseudorange rate errorm/s0.9Due to the high power consumption of SAR, we perform four imaging in the terminal phase, which provides four measurement information and is described in Figure5.Figure 5
The SAR positioning in the terminal phase. (a) The trajectory of vehicle flight. (b) The SAR image.
(a)(b)


## 3.2. Experiment Parameters

The setting of the initial value of the state covariance matrixP, the value of the process noise covariance matrix Q, and the value of the measurement noise covariance matrix R play an important role in the convergence and convergence speed of the filter.As for the 17-dimensional diagonal matrixP, the covariance of the velocity and position is set according to the initial error. The covariance of the attitude is set according to the statistical variance of the alignment result. The covariance of gyro and the accumulator bias are set according to their repeatability. The covariance of pseudorange error and the pseudorange rate error is set according to the clock error parameters and the clock rate error parameters of the measurement. Based on the rules above, in the flight experiment the initial value of the P is set to:
(18)P0=diag0.02;2;0.02∗π180;5;5;5;10006378137;10006378137;1000;0.13600;0.13600;0.13600∗π180;0.5;0.5;0.5∗9.8106;0.25;0.0025.Furthermore, bothQ and R are set to constant values. The component noise covariance corresponding to the attitude and velocity in the Q is set to the noise of the gyro and accelerometer separately, and the rest are small values. The component noise covariance corresponding to the pseudorange of the R matrix is set to 1, and the component corresponding to the pseudorange rate is set to 0.1.


## 3.3. Results

The experiment results of longitude error, latitude error, and altitude error are given in Figures6, 7, and 8, respectively. At 42 s, the GNSS signals began to be jammed. Thus, in the time range of [0, 41.9] s and [42.0, 50.0] s, we perform the tightly coupled SINS/GNSS and tightly coupled SINS/SAR/GNSS integration algorithm, respectively.Figure 6
Results of longitude error. (a) The longitude error in the whole phase. (b) The longitude error in the terminal phase.
(a)(b)Figure 7
Results of latitude error. (a) The latitude error in the whole phase. (b) The latitude error in the terminal phase.
(a)(b)Figure 8
Results of altitude error. (a) The altitude error in the whole phase. (b) The altitude error in the terminal phase.
(a)(b)As shown above, when the GNSS signal is interfered, the SINS/GNSS will cause an average position error of 2.8 m within 2 seconds, which is intolerable for supersonic vehicles in the terminal phase. At the moment of 43.9 s, 45.9 s, 47.9 s, and 49.9 s, the SINS/SAR/GNSS algorithm is utilized, and the average position error could be corrected to 1.7 m within 1 cycle. With the relatively few measurement times and the long measurement cycle, the measurement of SAR can suppress the divergence of SINS/GNSS positioning, and the proposed algorithm can achieve high positioning accuracy, which meets the requirements of supersonic vehicles.


## 4. Discussion

### 4.1. Discussion of the Experiment Results

The longitude and latitude error is shown in Figures7 and 8, separately. Due to the different initial values, we only show the details of the SINS/SAR/GNSS error without drawing the details of the SINS/GNSS error in the subfigure. However, it can be clearly seen from the comparison curve in the range of [42.0, 50.0] s that the SAR measurement plays an important role in correcting the positioning of the navigation system when the GNSS is interfered. At the moment of 43.9 s, 45.9 s, 47.9 s, and 49.9 s, when the SAR measurement comes, the SINS/SAR/GNSS algorithm is utilized, and the average position error could be corrected to 1.7 m within 1 cycle.As for the tightly SINS/SAR/GNSS integrated navigation system, there are few descriptions in the current published literature. At the same time, due to the uncertain delay of SAR measurement, few literatures have been found to propose a system solution. Therefore, this paper only compares the proposed method with the classical tightly SINS/GNSS integrated method.


### 4.2. Discussion of the Proposed System

The purpose of the proposed system is to design a SINS/SAR/GNSS integrated navigation system for the medium-range supersonic vehicle in the whole flight phase to tackle the interference of GNSS. In the early phase of the flight process, when the GNSS signal is not interfered with, the tightly SINS/GNSS integrated navigation system is used for navigation. Considering that the vehicle flies in its own territory in the early phase, the GNSS will not be interfered. Furthermore, the high power consumption of SAR only allows limited use, so SAR will not be used in this phase. In the terminal phase when the GNSS signal has interfered, the tightly SINS/SAR/GNSS integrated navigation system is performed.To cope with the SAR measurement delay, we design an approximate extrapolation algorithm under the framework of Kalman filter. This is an engineering solution, and the convergence of the method has not been proved theoretically. However, qualitative analysis can show that the approximate extrapolation algorithm has limited influence on the Kalman filter. This method will indeed bring about approximate errors which depend on the difference between the change of actual navigation information and the simulated linear change. In fact, considering that the attitude and velocity changes of the supersonic vehicle after the boost phase are small, the navigation change of the navigation state is small. Thus, the navigation state can be considered as linear, which is the basis of the application of this method. Moreover, it should be noted that this method can only be performed at discrete moments; thus, the shorter the SINS mechanization period, the higher the accuracy of the integrated navigation.


### 4.3. Discussion of the Future Work

According to Equation (11), when the SAR measurement fails and the GNSS is completely interfered with, the tightly SINS/SAR/GNSS integrated navigation system degenerates into a SINS navigation system. Actually, for general supersonic vehicles, after the boost phase, the velocity of the vehicle is within 900-1100 m/s, and the flight time is still about 160 s in the terminal phase of 120-200 Km. This allows sufficient time for SAR measurement, and it will generally return 4-25 effective measurements in the terminal phase, which is sufficient for the integrated system. Thus, the most important thing is the matching accuracy, which determines the positioning accuracy of the vehicle. At the same time, according to our experiments, different SAR matching rules should be designed in different environments to improve the matching accuracy, such as towns, rivers, mountains, plateaus, and other places.Therefore, in the future, we will study SAR matching and positioning in various complex environments and, at the same time, design automatic selection rules for different matching areas and increase the matching success rate to 99%. Furthermore, we will carry out research on the matching of SAR images and satellite images based on neural networks and, at the same time, explore its deployment on the vehicle processor.


## 4.1. Discussion of the Experiment Results

The longitude and latitude error is shown in Figures7 and 8, separately. Due to the different initial values, we only show the details of the SINS/SAR/GNSS error without drawing the details of the SINS/GNSS error in the subfigure. However, it can be clearly seen from the comparison curve in the range of [42.0, 50.0] s that the SAR measurement plays an important role in correcting the positioning of the navigation system when the GNSS is interfered. At the moment of 43.9 s, 45.9 s, 47.9 s, and 49.9 s, when the SAR measurement comes, the SINS/SAR/GNSS algorithm is utilized, and the average position error could be corrected to 1.7 m within 1 cycle.As for the tightly SINS/SAR/GNSS integrated navigation system, there are few descriptions in the current published literature. At the same time, due to the uncertain delay of SAR measurement, few literatures have been found to propose a system solution. Therefore, this paper only compares the proposed method with the classical tightly SINS/GNSS integrated method.


## 4.2. Discussion of the Proposed System

The purpose of the proposed system is to design a SINS/SAR/GNSS integrated navigation system for the medium-range supersonic vehicle in the whole flight phase to tackle the interference of GNSS. In the early phase of the flight process, when the GNSS signal is not interfered with, the tightly SINS/GNSS integrated navigation system is used for navigation. Considering that the vehicle flies in its own territory in the early phase, the GNSS will not be interfered. Furthermore, the high power consumption of SAR only allows limited use, so SAR will not be used in this phase. In the terminal phase when the GNSS signal has interfered, the tightly SINS/SAR/GNSS integrated navigation system is performed.To cope with the SAR measurement delay, we design an approximate extrapolation algorithm under the framework of Kalman filter. This is an engineering solution, and the convergence of the method has not been proved theoretically. However, qualitative analysis can show that the approximate extrapolation algorithm has limited influence on the Kalman filter. This method will indeed bring about approximate errors which depend on the difference between the change of actual navigation information and the simulated linear change. In fact, considering that the attitude and velocity changes of the supersonic vehicle after the boost phase are small, the navigation change of the navigation state is small. Thus, the navigation state can be considered as linear, which is the basis of the application of this method. Moreover, it should be noted that this method can only be performed at discrete moments; thus, the shorter the SINS mechanization period, the higher the accuracy of the integrated navigation.


## 4.3. Discussion of the Future Work

According to Equation (11), when the SAR measurement fails and the GNSS is completely interfered with, the tightly SINS/SAR/GNSS integrated navigation system degenerates into a SINS navigation system. Actually, for general supersonic vehicles, after the boost phase, the velocity of the vehicle is within 900-1100 m/s, and the flight time is still about 160 s in the terminal phase of 120-200 Km. This allows sufficient time for SAR measurement, and it will generally return 4-25 effective measurements in the terminal phase, which is sufficient for the integrated system. Thus, the most important thing is the matching accuracy, which determines the positioning accuracy of the vehicle. At the same time, according to our experiments, different SAR matching rules should be designed in different environments to improve the matching accuracy, such as towns, rivers, mountains, plateaus, and other places.Therefore, in the future, we will study SAR matching and positioning in various complex environments and, at the same time, design automatic selection rules for different matching areas and increase the matching success rate to 99%. Furthermore, we will carry out research on the matching of SAR images and satellite images based on neural networks and, at the same time, explore its deployment on the vehicle processor.


## 5. Conclusions

Since GNSS signals are easily interfered, this paper proposes a tightly coupled SINS/SAR/GNSS integration algorithm, which can complete the navigation of supersonic vehicles in the whole flight phase even if the GNSS signal is interfered completely. In the boost and midcourse phase, we use the tightly coupled SINS/GNSS integration as the navigation method. In the terminal phase, we introduce the SAR-aided navigation system and proposed a tightly coupled SINS/SAR/GNSS integration algorithm to cope with the interference of the GNSS. In order to accelerate the speed of SAR positioning and improve the accuracy of position at the same time, we design a positioning method based on the area feature and SSDA. The comparison experiment shows that our method possesses the highest accuracy and speed. Finally, the experiment results show that the proposed tightly coupled SINS/SAR/GNSS integration algorithm can correct the average position error to 1.7 m within 1 cycle. In short, the proposed SINS/SAR/GNSS algorithm can complete the navigation in the whole phase and suppress the divergence in terminal phase compared with conventional SINS/GNSS.



---

*Source: 1560795-2022-04-13.xml*