# A SINS/BDS Integrated Navigation Method Based on Classified Weighted Adaptive Filtering

Authors: Xuchao Kang; Guangjun He; Xingge Li
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

Aiming at the problem that the accuracy and stability of SINS/BDS integrated navigation system decrease due to uncertain model and observation anomalies, a SINS/BDS integrated navigation method based on classified weighted adaptive filtering is proposed. Firstly, the innovation covariance matching technology is used to detect whether there is any abnormality in the system as a whole. Then the types of anomalies are distinguished by hypothesis test. Different types of anomalies have different effects on state estimation. Based on the dynamic changes of innovation, different adaptive weighting methods are adopted to correct navigation information. The simulation results show that this method can effectively improve the fault-tolerant performance of integrated navigation system in complex environment with unknown anomaly types. When both model anomalies and observation anomalies exist, the speed and position accuracy are increased by 42% and 24% compared with the standard KF, 38% and 22% compared with the innovation orthogonal adaptive filtering, which has higher navigation accuracy.


---

## Body

## 1. Introduction

The development of high-speed cruise aircraft puts higher demands on the accuracy and fault tolerance of navigation systems [1]. SINS/BDS integrated navigation system combines the advantages of each sub-navigation system through filtering algorithm. It overcomes the defects that the output information errors of the SINS accumulate over time and the measurements of BDS are easily affected by the environment. Therefore, SINS/BDS has been widely used in the design of aircraft navigation systems [2–4]. However, with the development of hypersonic vehicles, the rapid increase of flight speed makes the flight environment of the aircraft change rapidly in a short time; it will cause the aircraft to be disturbed by more uncertain factors. These uncertainties will make the navigation system anomalies occur and navigation errors increase or even diverge.The online matching technology of innovation and its covariance matrix is widely used in the anomaly detection of navigation systems. Many literatures have carried out in-depth research and improvement [5–8]. In [5], an adaptive filtering algorithm based on the orthogonality of innovation is proposed. According to the orthogonality of innovation, the outliers are identified and weighted to weaken the influence of outliers on filtering. But it uses the same weighting factor for different outlier signals; the overall filtering accuracy is not high. In [6], the anomalous signals are classified by comparing the theoretical and actual values of the innovation covariance. Then different weighting matrices are used to correct the filter deviation caused by the outliers. It improves the accuracy of the navigation system, but does not consider the influence of model error caused by uncertain factors on the filtering results. In reference [7], a filtering method with adaptive factor is proposed. It is considered that the covariance matrix of residual sequence is mainly affected by the covariance matrix of system noise. The adaptive factor is constructed to adjust the system noise matrix in real time to reduce the influence of uncertain dynamic model errors on navigation filtering solution. However, the influence of measurement anomalies on residual covariance matrix is not considered in this paper. When measurement anomalies occur, filtering errors will tend to increase.The innovation variance can be used to test whether the system is abnormal. However, due to the uncertainty of interference factors, it is impossible to determine whether the anomaly that occurred is model anomaly or measurement anomaly. For the above problem, the previous references did not do too much analysis, but designed the fault-tolerant filtering algorithm by simply considering the innovation covariance matching error as a known type of anomaly. However, in the case where the type of the anomaly is uncertain, this method will seriously affect the accuracy of the navigation information and even produce the opposite fault-tolerant effect. So a SINS/BDS integrated navigation method based on classification weighted adaptive filtering is designed in this paper. Firstly, according to the orthogonality of the innovation, the method uses the innovation covariance matching technology to identify whether the system is abnormal. Based on the innovation covariance, the weighting factors are constructed. Then the type of the anomaly is further tested. According to the test results, different weighting methods are adopted to correct the filtering error caused by the uncertain factors. In addition, the vector-form weighting factors are used instead of the scalar-form weighting factors in the weighting process to further improve the accuracy of the navigation filtering.


## 2. SINS/BDS Integrated Navigation Mathematical Model

### 2.1. Integrated Navigation System State Equation

The ENU geography coordinate system is selected as the basic coordinate system of the navigation solution, and the state equation is established as formula (1)(1)Xk+1=FkXk+GkWkSystem state variable is expressed as(2)X=δL,δλ,δh,δVe,δVn,δVu,ϕe,ϕn,ϕu,εx,εy,εz,∇x,∇y,∇zTwhere δL,δλ,δh represent latitude error, longitude error and height error respectively; δVe,δVn,δVu represent the velocity error in the east, north and up directions respectively; ϕe,ϕn,ϕu are the mathematical platform angular error. εx,εy,εz are the gyro random constant drift. ∇x,∇y,∇z are the accelerometer random constant value deviation. W=[wεx,wεy,wεz,w∇x,w∇y,w∇z]T represents integrated navigation system noise.


### 2.2. Integrated Navigation System Measurement Equation

Take the difference between the position and velocity information of the INS and the BDS as the observation measurement, and establish the system measurement equation as formula (3)(3)Zk=HpHvXk+VpkVvkHp and Hv are measurement matrices of measurement equation, which are expressed as Hp=[diag(RM,RNcos⁡L,1),03×6,03×6], Hv=[03×3,diag1,1,1,03×9)]. Vp and Vv are the position and velocity measurement noise of BDS in the ENU geography coordinates system, which can be expressed as Vp=[NGe,NGn,NGu]T, Vv=[vGe,vGn,vGu]T.


## 2.1. Integrated Navigation System State Equation

The ENU geography coordinate system is selected as the basic coordinate system of the navigation solution, and the state equation is established as formula (1)(1)Xk+1=FkXk+GkWkSystem state variable is expressed as(2)X=δL,δλ,δh,δVe,δVn,δVu,ϕe,ϕn,ϕu,εx,εy,εz,∇x,∇y,∇zTwhere δL,δλ,δh represent latitude error, longitude error and height error respectively; δVe,δVn,δVu represent the velocity error in the east, north and up directions respectively; ϕe,ϕn,ϕu are the mathematical platform angular error. εx,εy,εz are the gyro random constant drift. ∇x,∇y,∇z are the accelerometer random constant value deviation. W=[wεx,wεy,wεz,w∇x,w∇y,w∇z]T represents integrated navigation system noise.


## 2.2. Integrated Navigation System Measurement Equation

Take the difference between the position and velocity information of the INS and the BDS as the observation measurement, and establish the system measurement equation as formula (3)(3)Zk=HpHvXk+VpkVvkHp and Hv are measurement matrices of measurement equation, which are expressed as Hp=[diag(RM,RNcos⁡L,1),03×6,03×6], Hv=[03×3,diag1,1,1,03×9)]. Vp and Vv are the position and velocity measurement noise of BDS in the ENU geography coordinates system, which can be expressed as Vp=[NGe,NGn,NGu]T, Vv=[vGe,vGn,vGu]T.


## 3. Classification Weighted Adaptive Filtering Algorithm

### 3.1. Problem Analysis

Innovationek can be described as ek=Zk-HX^k.k-1; it indicates the deviation between the real measurement and prediction of navigation system. The abnormality of navigation system will cause the change of innovation statistical characteristics [9]. According to the orthogonality theory of innovation, it can be deduced that the theoretical innovation covariance matrix can be expressed as equation (4).(4)Lk=EekekT=EZk-HX^k,k-1Zk-HX^k,k-1T=EHXk+Vk-HX^k,k-1HXk+Vk-HX^k,k-1T=EHX~k+VkHX~k+VkT=HPk,k-1HT+Rkwhere X~k is the system one-step prediction error and Vk is the measurement error. The innovation covariance matching algorithm compares the theoretical and actual values of the innovation covariance and judges the system to be abnormal when the actual value deviates significantly from the theoretical value [9].When the uncertainty causes the navigation system to measure abnormally, the measurement equation can be expressed as formula (5). (5)Zk∗=HXk+Vk+∆Vkwhere Zk∗ denotes abnormal measurement; Vk denotes fixed measurement noise, and ∆Vk denotes uncertain measurement noise that causes abnormal measurement. Innovation covariance is expressed as equation (6). (6)EekekT=HX~k+Vk+∆VkHX~k+Vk+∆VkT=HPk,k-1HT+Rk+∆RkWhen the uncertainty causes the system model of the navigation system to be abnormal, the equation of state can be expressed as equation (7) (7)Xk∗=F+∆FXk-1+GkWkState one-step prediction equation is expressed as equation (8) (8)X^k,k-1∗=φ+∆φXk-1=X^k-1+∆X^k-1X^k,k-1∗ represents the one-step prediction of state variable when the model has an abnormality, ∆φ represents the model error of the abnormal system. Innovation covariance is expressed as formula (9) (9)EekekT=EHXk-X^k,k-1-∆X^k,k-1+VkHXk-X^k,k-1-∆X^k,k-1+VkT=HPk,k-1HT+H∆QkHT+RkBased on the above analysis, the actual innovation covariance will deviate from the theoretical value regardless of whether the model is abnormal or the measurement is abnormal. The covariance matching method can be used to determine whether the system has an abnormality, but the type of the anomaly cannot be determined. In theory, when the measurement is abnormal, the gain is usually reduced to weaken the influence of the abnormal measurement on the filtering. When the model is abnormal, the gain is usually increased to enhance the correction effect of the measurement on the model error [10]. Therefore, in the case where the type of anomaly cannot be determined, the traditional single weighted filtering method may produce the opposite fault-tolerant effect.


### 3.2. Classified Weighted Adaptive Filtering Algorithm

First of all, the one-step prediction error equation can be described as follows.(10)Pk,k-1=φk,k-1Pk-1φk,k-1T+Γk-1Qk-1Γk-1TFrom equation (10), it can be seen that when the system model is abnormal or the statistical characteristics of the system noise change, equation (11) can be obtained and the one-step prediction variance will change.(11)Pk,k-1=φk,k-1+∆φk,k-1Pk-1φk,k-1+∆φk,k-1T+Γk-1Qk-1+∆Qk-1Γk-1TTherefore, this paper simplifies the correction of the navigation system anomaly caused by the uncertainty model error into the adaptive adjustment of the one-step prediction variance matrix. Through the adaptive weighting of the measurement noise, the fault tolerance of the navigation system is measured. Filter gain and state estimation equation can be obtained as follows.(12)Kk=βkPk,k-1HkTHkβkPk,k-1HkT+αkRk-1(13)X^k=X^k,k-1+KkekFor the calculation of the weighting factors α and β, different calculation methods will be adopted for different types of abnormalities after judging that there is an abnormality in the whole system.Firstly, the innovation covariance matching method is used to judge whether the system as a whole is abnormal. For the calculation of innovation covariance, a calculation method with sliding window can be used [11].(14)L^k=k-1kL^k-1+1kekekTk≤N1N∑i=k-N+1keieiTk>N(15)L-k=k-1kL-k-1+1kHkPk,k-1HkT+Rkk≤N1N∑i=k-N+1kHiPi,i-1HiT+Rik>NN is a sliding window, we can adjust the size of N to achieve different requirements for the sensitivity and accuracy of the filter. L^k is the actual value of the innovation variance, L-k is the theoretical value of the innovation variance. When L^k deviates from L-k seriously, there are abnormalities in the system.After the above judgment, when the whole system is abnormal; the next hypothesis test is carried out:H0: There are anomalies in the measurements.H1: There are anomalies in the system model.The measurement error equation can be expressed as equation (16).(16)Vk=HkX^k-ZkVk can reflect the error of observation. We construct the anomaly factor η as equation (17)(17)η=trVkVkTtr∑Vkwhere ∑Vk is the covariance matrix of Vk [12].Whenη>λ, the measurement of navigation system is abnormal. λ is the abnormality detection threshold, and the range is usually taken as 1.03.0. We build the error function Jαi as follows.(18)Jαi=∑L^ki,iGk,k-1i,i+αiRi,i-12Among them, Gk,k-1=HkPk,k-1HkT. The necessary and sufficient condition for Jαi to get the minimum value is αki=L^ki,i-Gk,k-1i,i/Rki,i. Then we construct the weighted factors as follows:(19)αk=diagαk1,αk2⋯αknβk=Im×mAmong them, n is the observation dimension and m is the state variable dimension.Otherwise, we regard the anomaly of navigation system as model anomaly and establish error functionJβi as follows.(20)Jβi=∑L^ki,iβiGk,k-1i,i+Rki,i-12The necessary condition for Jβi to get the minimum value is βki=L^ki,i-Rki,i/Gk,k-1i,i. To ensure the consistency of the matrix dimensions during the calculation of the filter gain, the weighting factors are constructed as follows:(21)αk=In×nβk=diagβk1,βk2⋯βkn,1⋯1m×m


## 3.1. Problem Analysis

Innovationek can be described as ek=Zk-HX^k.k-1; it indicates the deviation between the real measurement and prediction of navigation system. The abnormality of navigation system will cause the change of innovation statistical characteristics [9]. According to the orthogonality theory of innovation, it can be deduced that the theoretical innovation covariance matrix can be expressed as equation (4).(4)Lk=EekekT=EZk-HX^k,k-1Zk-HX^k,k-1T=EHXk+Vk-HX^k,k-1HXk+Vk-HX^k,k-1T=EHX~k+VkHX~k+VkT=HPk,k-1HT+Rkwhere X~k is the system one-step prediction error and Vk is the measurement error. The innovation covariance matching algorithm compares the theoretical and actual values of the innovation covariance and judges the system to be abnormal when the actual value deviates significantly from the theoretical value [9].When the uncertainty causes the navigation system to measure abnormally, the measurement equation can be expressed as formula (5). (5)Zk∗=HXk+Vk+∆Vkwhere Zk∗ denotes abnormal measurement; Vk denotes fixed measurement noise, and ∆Vk denotes uncertain measurement noise that causes abnormal measurement. Innovation covariance is expressed as equation (6). (6)EekekT=HX~k+Vk+∆VkHX~k+Vk+∆VkT=HPk,k-1HT+Rk+∆RkWhen the uncertainty causes the system model of the navigation system to be abnormal, the equation of state can be expressed as equation (7) (7)Xk∗=F+∆FXk-1+GkWkState one-step prediction equation is expressed as equation (8) (8)X^k,k-1∗=φ+∆φXk-1=X^k-1+∆X^k-1X^k,k-1∗ represents the one-step prediction of state variable when the model has an abnormality, ∆φ represents the model error of the abnormal system. Innovation covariance is expressed as formula (9) (9)EekekT=EHXk-X^k,k-1-∆X^k,k-1+VkHXk-X^k,k-1-∆X^k,k-1+VkT=HPk,k-1HT+H∆QkHT+RkBased on the above analysis, the actual innovation covariance will deviate from the theoretical value regardless of whether the model is abnormal or the measurement is abnormal. The covariance matching method can be used to determine whether the system has an abnormality, but the type of the anomaly cannot be determined. In theory, when the measurement is abnormal, the gain is usually reduced to weaken the influence of the abnormal measurement on the filtering. When the model is abnormal, the gain is usually increased to enhance the correction effect of the measurement on the model error [10]. Therefore, in the case where the type of anomaly cannot be determined, the traditional single weighted filtering method may produce the opposite fault-tolerant effect.


## 3.2. Classified Weighted Adaptive Filtering Algorithm

First of all, the one-step prediction error equation can be described as follows.(10)Pk,k-1=φk,k-1Pk-1φk,k-1T+Γk-1Qk-1Γk-1TFrom equation (10), it can be seen that when the system model is abnormal or the statistical characteristics of the system noise change, equation (11) can be obtained and the one-step prediction variance will change.(11)Pk,k-1=φk,k-1+∆φk,k-1Pk-1φk,k-1+∆φk,k-1T+Γk-1Qk-1+∆Qk-1Γk-1TTherefore, this paper simplifies the correction of the navigation system anomaly caused by the uncertainty model error into the adaptive adjustment of the one-step prediction variance matrix. Through the adaptive weighting of the measurement noise, the fault tolerance of the navigation system is measured. Filter gain and state estimation equation can be obtained as follows.(12)Kk=βkPk,k-1HkTHkβkPk,k-1HkT+αkRk-1(13)X^k=X^k,k-1+KkekFor the calculation of the weighting factors α and β, different calculation methods will be adopted for different types of abnormalities after judging that there is an abnormality in the whole system.Firstly, the innovation covariance matching method is used to judge whether the system as a whole is abnormal. For the calculation of innovation covariance, a calculation method with sliding window can be used [11].(14)L^k=k-1kL^k-1+1kekekTk≤N1N∑i=k-N+1keieiTk>N(15)L-k=k-1kL-k-1+1kHkPk,k-1HkT+Rkk≤N1N∑i=k-N+1kHiPi,i-1HiT+Rik>NN is a sliding window, we can adjust the size of N to achieve different requirements for the sensitivity and accuracy of the filter. L^k is the actual value of the innovation variance, L-k is the theoretical value of the innovation variance. When L^k deviates from L-k seriously, there are abnormalities in the system.After the above judgment, when the whole system is abnormal; the next hypothesis test is carried out:H0: There are anomalies in the measurements.H1: There are anomalies in the system model.The measurement error equation can be expressed as equation (16).(16)Vk=HkX^k-ZkVk can reflect the error of observation. We construct the anomaly factor η as equation (17)(17)η=trVkVkTtr∑Vkwhere ∑Vk is the covariance matrix of Vk [12].Whenη>λ, the measurement of navigation system is abnormal. λ is the abnormality detection threshold, and the range is usually taken as 1.03.0. We build the error function Jαi as follows.(18)Jαi=∑L^ki,iGk,k-1i,i+αiRi,i-12Among them, Gk,k-1=HkPk,k-1HkT. The necessary and sufficient condition for Jαi to get the minimum value is αki=L^ki,i-Gk,k-1i,i/Rki,i. Then we construct the weighted factors as follows:(19)αk=diagαk1,αk2⋯αknβk=Im×mAmong them, n is the observation dimension and m is the state variable dimension.Otherwise, we regard the anomaly of navigation system as model anomaly and establish error functionJβi as follows.(20)Jβi=∑L^ki,iβiGk,k-1i,i+Rki,i-12The necessary condition for Jβi to get the minimum value is βki=L^ki,i-Rki,i/Gk,k-1i,i. To ensure the consistency of the matrix dimensions during the calculation of the filter gain, the weighting factors are constructed as follows:(21)αk=In×nβk=diagβk1,βk2⋯βkn,1⋯1m×m


## 4. Simulation Experiment and Analysis

The simulation parameters are set as follows:The initial position of the aircraft is the north latitude 31.9°, the east longitude 106.5° and the height 1000m. The initial speed is 100m/s. The yaw angle, pitch angle, and the roll angle are 30°, 0°, 0° respectively. The error gyro constant drift is 0.1°/h(1σ) and the accelerometer bias is 100ug (1 σ). The initial position error is 10m, the speed error is 0.8m/s, and the output frequency of SINS is 100Hz. The root mean square of BDS position information error is 20m, the root mean square of velocity information error is 0.5m/s, the output period is 1s, and the simulation time is 1800s. In order to fully verify the influence of the algorithm on the integrated navigation performance, the flight status of the aircraft should include climbing, dive, acceleration, cruise and steering. The simulation flight trajectory is shown in Figure 1.Figure 1
Flight path of the aircraft.In order to verify the performance of the classified weighted adaptive filter (CWAF) proposed in this paper, it is assumed that both model and measurement anomalies exist in INS/BDS integrated navigation system during the simulation period. In the simulation time of 500s-520s, we set up anomalous measurements. In the time of 500s-510s, the mean square root of velocity measurement errors in east, north and up directions are 2m/s, -3m/s and 2m/s respectively. In 510s-520s, the velocity errors are 3m/s, -1m/s and-3m/s respectively. We set abnormal model errors in 1000s-1020s. For convenience of observation, we set the root mean square of one-step prediction position error and velocity error caused by abnormal system model as 2m, 2m, -2m, 0.2m/s, 0.2m/s and -0.2m/s, respectively. In this paper, the standard Kalman filter and the innovation orthogonality adaptive filtering algorithm(IOAF) proposed in [5] are used for simultaneous simulation. The simulation results are shown in Figures 2–7.Figure 2
Eastward velocity error comparison.Figure 3
Northward velocity error comparison.Figure 4
Upward velocity error Comparison.Figure 5
Latitude error comparison.Figure 6
Longitude error comparison.Figure 7
Height error comparison.It can be seen from Figures2–7 that the standard KF has large fluctuations when the measurement is abnormal during the 500s-520s period, and the fluctuation amplitude of the velocity and position errors reaches 2.2m/s and 38m. Compared with the standard KF, the IOAF and the CWAF proposed in this paper have better fault tolerance. The velocity and position errors are more stable and keeping within 1m/s and 22m respectively. When the model is abnormal during the 1000s-1020s period, the filtering errors of the standard KF and the IOAF fluctuate greatly. The error amplitude of standard KF reach 2m/s and 36m. The IOAF has poor fault tolerance, and the errors reach 2.3m/s and 38m. In contrast, the CWAF can effectively suppress the influence of model anomalies on the filtering solution. The velocity and position errors are controlled within 0.9m/s and 20m. Navigation information error statistics are shown in Table 1.Table 1
Navigation information error data statistics.
Statistics Filtering method V e/(m/s) V n/(m/s) V u/(m/s) L/m λ /m h /m Standard deviation Standard KF 0.289 0.277 0.329 8.429 6.885 7.842 IOAF 0.277 0.287 0.283 7.237 7.177 7.788 CWAF 0.166 0.144 0.213 5.319 5.177 7.211In Table1, we compare the standard deviation of velocity and position error obtained by using three filtering methods in the presence of uncertain anomalies. Through analysis, it can be seen that the proposed CWAF has better fault-tolerant effect on the whole. And it can obtain higher precision navigation information than the standard KF and the IOAF when the anomaly type of navigation system is unknown due to uncertainties. The accuracy of velocity and position are 42% and 24% higher than that of standard KF respectively, 38% and 22% higher than that of IOAF; the error results are more stable.


## 5. Conclusions

Aiming at the problem that the filtering accuracy and stability of SINS/BDS integrated navigation system decrease due to uncertain model and observation anomalies, a new SINS/BDS integrated navigation method based on classified weighted adaptive filtering is proposed. Firstly, the types of anomalies in navigation system are distinguished by this method. Then, based on the analysis of the influence of model anomaly and observation anomaly on navigation filtering solution, different weighting methods are adopted for two types of system anomalies to improve the accuracy and adaptability of integrated navigation system. Finally, the simulation results show that the proposed method can effectively improve the fault-tolerant performance of integrated navigation system in complex environments with unknown types of anomalies. When both model and observation anomalies exist, the velocity and position accuracy of SINS/BDS integrated navigation system are improved by 42% and 24% compared with the standard KF, 38% and 22% compared with the innovation orthogonal adaptive filtering. It greatly improves the accuracy of SINS/BDS integrated navigation system and has high fault-tolerance and feasibility in complex environment with uncertainties.



---

*Source: 2158351-2019-07-07.xml*