# Improved Auxiliary Particle Filter for SINS/SAR Navigation

Authors: Li Xue; Chunning Na; Yulan Han
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

In order to obtain the relatively appropriate importance density function and alleviate the problem of particle degradation, a new improved auxiliary particle filter algorithm is proposed. After calculating the auxiliary variable, the adaptive regulator is employed to obtain the state estimation. So, the latest measurement information is efficiently utilized to establish a better importance density function in the importance sampling process. Then, the process of particle weights’ adaptive adjustment and random-weighted calculation can keep the diversity of particles and improve the filter precision; thus, it can better solve the filter problem of nonlinear system model error and noise interference. The simulation and analysis result show that the proposed algorithm can optimize the filter performance and improve the calculation precision in the positioning of the SINS/SAR integrated navigation system, compared with the other two existing filters.


---

## Body

## 1. Introduction

The particle filter (PF) algorithm can use a series of weighted random sample sets in the state space to approximate the posterior probability density function of the system state, which is applicable to any nonlinear non-Gaussian system that can be represented by the state space models [1, 2]. The importance sampling process of the particle filter is simple and easy to calculate. However, the change of measurement information at the current moment is not considered when the prior distribution is selected as the important density function. This sampling process only extracts particle samples according to the particle motion and previous state, so it can make them too sensitive to outliers and lose a large number of low weight particles [3, 4]. At the same time, the increase in the variance of particle weight will cause serious particle degradation. There are two key techniques to mitigate the particle degradation that are selection of importance density function and resampling process [5, 6].Since the 1990s, the scholar Gordon proposed the particle filter algorithm [7]; this method has attracted great attention from many scholars around the world, which has also promoted the particle filter to improve continuously. Although it can generate many new developed algorithms based on particle filter, such as central difference particle filter (CDPF), unscented particle filter (UPF), marginalized particle filter (MPF), Gaussian particle filter (GPF), and auxiliary particle filter (APF). Inevitably, there are still many shortcomings and limitations in the sampling theory, method, and application of the particle filter. In different development particle filter algorithms, it is important to complete effectively importance sampling and alleviate weight degradation and sample depletion in the filter process. There is a general problem that needs to be solved urgently to obtain better filter performance under different model noises’ conditions. The CDPF method based on CDKF can approximate the posterior mean value and variance by numerical integration or polynomial interpolation. The UPF method based unscented transformation is proposed as an improvement to PF. It has been demonstrated in many literature that the UPF is generally superior to the PF in terms of accuracy. The unscented transformation can obtain a set of deterministic weighted sampling points by means of the state posterior probability distribution at the last moment, the mean and variance probability distribution of the process, and the observation noise at the current moment [8, 9]. There is no need to the linearization of the nonlinear function, and the calculation accuracy can reach at least the second order of the Taylor series expansion. However, in many application cases, CDPF, UPF, and so on are required to obey the Gaussian distribution and cause particle degradation [10].In 1999, Pitt et al. had proposed an auxiliary particle filter (APF) [11]. This method designs the sampling distribution by introducing an auxiliary variable and shows better performance compared with PF. But its improvement is limited, especially when these two kinds of problems occur: the state transfer density is very dispersed and the likelihood density changes significantly relative to the state transfer density [12]. If the prior probability density function is used for particle importance sampling and the current measurement is not taken into account, the APF could not significantly improve estimation accuracy in many engineering applications cases. The sampled particles will seriously deviate from the actual target position, thus reducing the accuracy of the state prediction mean value and covariance.In order to enhance the filter capability of particle filter algorithm for nonlinear environmental measurement data, the influence of measurement information, model error, and noise interference should also be fully considered in the design of importance density function during APF sampling and resampling process. So, this paper proposes a new improved auxiliary particle filter (IAPF), and it is applied to the navigation position system. This filter algorithm designs an appropriate importance density function to carry out importance sampling, which could take the latest measurement information into account and adjust adaptively the particle weight distribution. Then, these obtained state estimation and variance will perform random weighted calculation to improve the filter performance.With the rapid development of navigation technology, the demand for the reliability and high precision of the navigation system is getting much higher. The single navigation system can no longer meet the increased demand. So the integrated navigation system can get much attention. The strap-down inertial navigation system (SINS) has many advantages, especially high autonomy, strong concealment, continuous measurement carrier, and not easily disturbed by the outside interference. So, it occupies the core position in the navigation system application. Synthetic aperture radar (SAR) has the advantages of high resolution, long time operation, remote imaging, and detection of relatively hidden targets. However, it can easily be affected by coherent lights and so on. So, the SINS/SAR integrated navigation system is adopted in this paper. The proposed algorithm was applied to the SINS/SAR integrated navigation system and compared with the other filters.


## 2. Auxiliary Particle Filter Analysis

### 2.1. Nonlinear System Model and Particle Filter Algorithm

In practical engineering applications, many filter conditions are nonlinear environment, so the high-efficiency filters method suitable for nonlinear systems is very important. For the linear systems, the most commonly used solution of the optimal filter is the famous Kalman filter (KF). But for the nonlinear systems, it is difficult or even impossible to obtain the optimal filter solution, because it needs to deal with infinite-dimensional integral calculation. The PF can use sample sets’ form to describe prior information and posterior information, rather than function forms, which can deal with the problem of solving nonlinear filter. Consider the following nonlinear system:(1)xk=fxk−1,wk−1,yk=hxk,vk,where xk∈Rn is the state vector at epoch k, yk∈Rn is the system observation, wk−1∈Rn is the system state noise with variance Qk, vk∈Rn is the measurement noise with variance Rk, f⋅ and h⋅ are nonlinear functions, and k=0,1,…,N is the sampling time. The initial state obeying any distribution is Ex0=x¯0 denoting the initial mean and covx0=P0 denotes the initial covariance matrix.The PF approximates the probability density functionpxk|yk through a set of random samples and replaces the integral operation with the sample mean in the state space. Then, it can adjust continuously weights and positions of particles according to the observation information, to obtain the minimum variance of states estimation [13, 14]. The particle importance weight can be updated recursively by(2)wk=py1:k|x0:kpx0:kqxk|x0:k−1,y1:kqx0:k−1|y1:k−1=wk−1pyk|xkpxk|xk−1qxk|x0:k−1,yk,where pyk|xk denotes the likelihood density and pxk|xk−1 denotes the transition density of the state. When the importance density function only depends on the previous moment state, it can be expressed as qxk|x0:k−1,y1:k=qxk|xk−1,yk. At time k, the weighted posterior probability density can be expressed as(3)pxk|y1:k≈∑i=1Nw˜kiδxk−xki,where ∑i=1nwki=1, w˜ki is normalization, and δ. is the Dirac function [1].In the importance sampling process, the traditional PF algorithm selects the prior density as the importance density function and uses the transfer probability to obtain the new importance density function. However, it does not consider the influence of the latest measurement value and cannot approximate the real distribution function very well, so the filter accuracy is affected. In addition, when the likelihood distribution is located at the tail of the transfer prior distribution or the observation model has a high precision, many samples will become invalid samples due to the small normalized weight. Then, PF could show degeneration phenomenon and the sample diversity will decrease. The effective method is to increase the number of particles, but this method is unrealistic.


### 2.2. The Main Steps of Auxiliary Particle Filter Algorithm

The APF algorithm is approximated by the empirical distribution made by recursive computation ofN weighted particles. In this algorithm, sampled particles are obtained from the joint density function pxk,i|y1:k, where i is the auxiliary variable and represents the sampled particles xk−1i⟶xk that are available for xk. By Bayesian theory, the joint density function can be expressed as(4)pxk,i|y1:k∝pyk|xk,i,y1:k−1pxk,i|y1:k−1pi|y1:k−1=pyk|xkpxk|xk−1iwk−1i.When this algorithm is to select the importance function, the following proportions should be satisfied:(5)qxk,i|y1:k∝pyk|λkipxk|xk−1iwk−1i,where λki is the representation of certain features of xk, xk−1i is known by conditions, and λki could be the conditional mean calculated by λki=Exk|xk−1i. After the importance density function performs decomposition calculation, it can be seen that the sample and auxiliary variables xki,ij can be generated by the following two steps [15–17]:(1)
Sampling from previous indicators,ij∼pyk|λkiwk−1i(2)
Sampling from prior density underijgiven conditions, xki∼pxk|xk−1ijThe main steps of APF are as follows:(1)
Initialization: fork=0, draw N sampling points according to the initial prior information, x0i∼pxo, where i=1,2,…,N.(2)
Fork=1,2,3,…,N, calculate the particle sample λki∼pxk|xk−1i.(3)
Calculate the corresponding weights of auxiliary variable and normalize it as(6)wki∗=pyk|λkiwk−1i∗,w˜ki∗=wki∗∑j=1Npwkj∗.(4)
Use the resampling procedure in the PF filter algorithm to obtain news state estimation and particle index setsij.(5)
The basic PF step is used to calculate the predicted state values’ samplingxki∼pxk|xk−1ij and update the weights as(7)wki∗=wk−1ipyk|xkipxki|xk−1iqxki,λki|,y1:k,∝pyk|xkipyk|λki.According to the particle filter step, calculate the state estimation and return to Step (2).The APF differs from PF in that the resampling process is calculated before the state prediction and weight update steps. So, the particles to be sampled are intuitively pushed to the high likelihood region. Theoretically, it can obtain better filter performance when the process noise is reduced. However, when the conditional likelihood is not insensitive to the state, this filter slightly alleviates the lack of particle diversity. In addition to that, the auxiliary variables sampling process cannot fully consider the influence of the latest measurement, and it will easily cause the system model error. To solve the above problems, a new improved algorithm is proposed based on APF for nonlinear systems.


## 2.1. Nonlinear System Model and Particle Filter Algorithm

In practical engineering applications, many filter conditions are nonlinear environment, so the high-efficiency filters method suitable for nonlinear systems is very important. For the linear systems, the most commonly used solution of the optimal filter is the famous Kalman filter (KF). But for the nonlinear systems, it is difficult or even impossible to obtain the optimal filter solution, because it needs to deal with infinite-dimensional integral calculation. The PF can use sample sets’ form to describe prior information and posterior information, rather than function forms, which can deal with the problem of solving nonlinear filter. Consider the following nonlinear system:(1)xk=fxk−1,wk−1,yk=hxk,vk,where xk∈Rn is the state vector at epoch k, yk∈Rn is the system observation, wk−1∈Rn is the system state noise with variance Qk, vk∈Rn is the measurement noise with variance Rk, f⋅ and h⋅ are nonlinear functions, and k=0,1,…,N is the sampling time. The initial state obeying any distribution is Ex0=x¯0 denoting the initial mean and covx0=P0 denotes the initial covariance matrix.The PF approximates the probability density functionpxk|yk through a set of random samples and replaces the integral operation with the sample mean in the state space. Then, it can adjust continuously weights and positions of particles according to the observation information, to obtain the minimum variance of states estimation [13, 14]. The particle importance weight can be updated recursively by(2)wk=py1:k|x0:kpx0:kqxk|x0:k−1,y1:kqx0:k−1|y1:k−1=wk−1pyk|xkpxk|xk−1qxk|x0:k−1,yk,where pyk|xk denotes the likelihood density and pxk|xk−1 denotes the transition density of the state. When the importance density function only depends on the previous moment state, it can be expressed as qxk|x0:k−1,y1:k=qxk|xk−1,yk. At time k, the weighted posterior probability density can be expressed as(3)pxk|y1:k≈∑i=1Nw˜kiδxk−xki,where ∑i=1nwki=1, w˜ki is normalization, and δ. is the Dirac function [1].In the importance sampling process, the traditional PF algorithm selects the prior density as the importance density function and uses the transfer probability to obtain the new importance density function. However, it does not consider the influence of the latest measurement value and cannot approximate the real distribution function very well, so the filter accuracy is affected. In addition, when the likelihood distribution is located at the tail of the transfer prior distribution or the observation model has a high precision, many samples will become invalid samples due to the small normalized weight. Then, PF could show degeneration phenomenon and the sample diversity will decrease. The effective method is to increase the number of particles, but this method is unrealistic.


## 2.2. The Main Steps of Auxiliary Particle Filter Algorithm

The APF algorithm is approximated by the empirical distribution made by recursive computation ofN weighted particles. In this algorithm, sampled particles are obtained from the joint density function pxk,i|y1:k, where i is the auxiliary variable and represents the sampled particles xk−1i⟶xk that are available for xk. By Bayesian theory, the joint density function can be expressed as(4)pxk,i|y1:k∝pyk|xk,i,y1:k−1pxk,i|y1:k−1pi|y1:k−1=pyk|xkpxk|xk−1iwk−1i.When this algorithm is to select the importance function, the following proportions should be satisfied:(5)qxk,i|y1:k∝pyk|λkipxk|xk−1iwk−1i,where λki is the representation of certain features of xk, xk−1i is known by conditions, and λki could be the conditional mean calculated by λki=Exk|xk−1i. After the importance density function performs decomposition calculation, it can be seen that the sample and auxiliary variables xki,ij can be generated by the following two steps [15–17]:(1)
Sampling from previous indicators,ij∼pyk|λkiwk−1i(2)
Sampling from prior density underijgiven conditions, xki∼pxk|xk−1ijThe main steps of APF are as follows:(1)
Initialization: fork=0, draw N sampling points according to the initial prior information, x0i∼pxo, where i=1,2,…,N.(2)
Fork=1,2,3,…,N, calculate the particle sample λki∼pxk|xk−1i.(3)
Calculate the corresponding weights of auxiliary variable and normalize it as(6)wki∗=pyk|λkiwk−1i∗,w˜ki∗=wki∗∑j=1Npwkj∗.(4)
Use the resampling procedure in the PF filter algorithm to obtain news state estimation and particle index setsij.(5)
The basic PF step is used to calculate the predicted state values’ samplingxki∼pxk|xk−1ij and update the weights as(7)wki∗=wk−1ipyk|xkipxki|xk−1iqxki,λki|,y1:k,∝pyk|xkipyk|λki.According to the particle filter step, calculate the state estimation and return to Step (2).The APF differs from PF in that the resampling process is calculated before the state prediction and weight update steps. So, the particles to be sampled are intuitively pushed to the high likelihood region. Theoretically, it can obtain better filter performance when the process noise is reduced. However, when the conditional likelihood is not insensitive to the state, this filter slightly alleviates the lack of particle diversity. In addition to that, the auxiliary variables sampling process cannot fully consider the influence of the latest measurement, and it will easily cause the system model error. To solve the above problems, a new improved algorithm is proposed based on APF for nonlinear systems.


## 3. A New Improved Auxiliary Particle Filter Algorithm

Because the APF algorithm is widely applied and it is crucial to design an appropriate importance density function, therefore, the latest measurement information, model error, and noise interference should be fully taken into account when the importance sampling and resampling process of the APF will be improvement and optimization. This paper proposes the IAPF algorithm that can adaptively adjust the state estimation and consider the measurement information change after calculating auxiliary variables; then, the importance density function will be close to the true distribution obtained by importance sampling. The particles’ weights are selected and adjusted adaptively, and, finally, the state estimation and variance are calculated by random weighted, so that the filter accuracy can be improved. The IAPF algorithm includes the following steps:Step 1 (initialization): fork=0, x0i∼px0, where i=1,2,…,N, assume the initial values x¯0i=Ex0i and P0i=Ex0i−x^0ix0i−x^0iT.Step 2: calculate particle index setsij, j=1,2,…,N, and λki∼pxk|xk−1i.Step 3: calculate the corresponding particle weights ofxki,ijj=1N and normalized weights.Then, a new particle set is obtained by auxiliary resampling,x¯ki,1/N∼xki,w˜ki∗.Step 4: if the distribution of these weighted particle sets is used to approximate the importance density function, i.e.,(8)qxk,i|y1:k=qi|y1:kqx¯k|i,y1:k,(9)qx¯k|i,y1:k≜pxk|xk−1i,the traditional auxiliary variable is only to be sampled from the transition density distribution. When the process noise is small, the performance of APF is usually better than that of the PF method; however, when there appears large variation of measurement information and noise changes in the system model, the sampled particle does not provide sufficient information and will seriously deviate from the actual target position, thus reducing the accuracy of the state predicted value and covariance. Therefore, the IAPF should not only fully consider the influence of the latest measurement information but also adaptively adjust parameters to adapt to model errors and noise interference.(1)
When the interval of observation time increases, the system model error will also increase. If the target state is not updated for a long time, it is difficult to accurately estimate the system motion model with limited observation information.Therefore, fori=1,2,…,N,(10)xki=x¯ki+αkiyki−hx¯ki,where the adaptive regulatorαki can be expressed as(11)αki=T2⋅v2⋅σv2kμ⋅σm2k+T2⋅v2⋅σv2k,whereμ is a constant, T denotes the time interval, vdenotes the position target velocity, σv2k denotes the standard deviation of measurement residual, σm2k denotes the standard deviation of measurement error, σm2k, and xki is generated based on the prior particle and the measurement information at time k, which is closer to the real state value [18, 19].This step can use many iterative computations and adaptive regulator to update the state estimation that will be to approximate the nonlinear observation, which further reduces the state estimation error and thus optimizes the filter estimation to obtain the importance density function, soxki,Pki is close to Nxki,Pki.(2)
Importance sampling (xki∼Nxki,Pki): calculate the particle weights:Step5: residual resampling.All particle weights are evaluated separately to generate new samples with similar weights. This process sometimes reduces the diversity of particles, which can be selected according to the specific situation. The weight is improved by Euclidean distance [20, 21], such as(12)Lmax=yk−mini−yk−maxiT⋅yk−mini−yk−maxi,(13)Li=yki−yk−maxiT⋅yki−yk−maxi,whereLmaxand Li denote the Euclidean distance of the measurement residual yk−maxi and yk−mini:(14)wki∗=wki∗+wk−maxi∗N⋅sinLiLmax⋅π/2⋅β,wherewk−maxi∗ denotes the maximum weight value, β=K/α∗ is the adaptive coefficient, K is the proportion constant value, and α∗ is determined by the statistical characteristics of the measurement noise. Then, normalized weights(15)w˜ki=wki∑i=1nwki∗.In the resampling process, we can getM particle again.Step 6: random-weighted calculation.The particles carried out a random weighted calculation, and the random weighted vectorsη1,η2,…,ηN all follow the Dirichlet distribution D1,…,1 [22] and satisfy ∑n=1Nηn=1. Calculate the state estimation and variance such as(16)x^k=∑n=1Nηnxkn,(17)P^k=∑n=1NηnP^kn.Return to Step (2) to recursively calculate the state estimation and variance based on the new measurement information.This algorithm can recursively calculate the sample particles through the adaptive regulator. It has a better adaptive ability to deal with measurement information and other system interference factors, and the residual resampling and random weighted calculation process makes the filter algorithm have better adaptability and robustness.


## 4. Simulation Results and Analysis

### 4.1. SINS/SAR Integrated Navigation System

The simulation experiment has been conducted to comprehensively evaluate and analyze the performance of the proposed algorithm for SINS/SAR integrated navigation system, and the comparison with the other two existing filters APF and PF is also discussed in this section. The SINS/SAR integrated navigation system makes use of the complementary characteristics of the SINS and SAR to improve the performance of the whole system [23, 24]. The schematic diagram of the SINS/SAR integrated navigation system is as in Figure 1.Figure 1
The schematic diagram of the SINS/SAR integrated navigation system.The local navigation frame (n-frame) is selected as the east-north-up (e-n-u) geography frame. Denote the inertial frame by i, the Earth frame by e, and the body frame by b. The dynamic model of the SINS/SAR integration navigation system can be expressed as(18)x˙=fx,t+wt,zt=hxt+vt,where fx,t is the system nonlinear state function, wt is the system process noise vector, h is the measurement matrix, vt is the measurement of white noise, and the mean value is zero. We define the system state vector as(19)xt=ϕe,ϕn,ϕu,δve,δvn,δvu,δL,δλ,δh,εxb,εyb,εzb,∇xb,∇yb,∇zbT,where ϕe,ϕn,andϕu denote attitude error quaternion of SINS, δve,δvn,andδvu denote velocity error in three directions of SINS, δL,δλ,andδh denote the errors of latitude, longitude, and height, εxb,εyb,andεzb denote random drift of the gyro, ∇xb,∇yb,and∇zb denote constant bias of the accelerometer, and wt is such as(20)wt=−Cω−1Cbn′ωabT,CbnωgbT,01×9T,whereCw−1,Cbn′, andCbn represent different rotation matrices. The attitude, velocity, and position error equations of SINS are given by [25, 26]. The system state equation in discrete-time form can be expressed as(21)xk=fxk−1+wk,where f⋅ is a discrete-time nonlinear function describing the dynamics of system state and wk is the discrete-time process noise [27].In this integrated system, the horizontal direction measurement is composed of the difference of heading angle, longitude, and latitude between SINS and SAR. The altitude direction measurement is composed of the difference of height between the SINS and the barometric altimeter. The system measurement equation in discrete-time form can be expressed as(22)zk=Hkxk+vk,and take the measurement vector as(23)zk=ψI−ψS,LI−LS,λI−λS,hI−heT,where ψI,LI,λI, and hI are, respectively, the heading angle and the position information for SINS; ψS,LS,andλS are the heading angle, latitude, and longitude position information for SAR. Due to the instability of the SINS system altitude measurement and the fact that the barometric altimeter is introduced to measure height information with he, δ denotes the corresponding error. zk can be rewritten as(24)zk=ψI−ψSLI−LSλI−λShI−he=ψ+δψ−ψ+δψSL+δL−L+δLSλ+δλ−λ+δλSh+δh−h+δhe=δψδLδλδh−δψSδLSδλSδhe.


### 4.2. Simulation Analysis

Assume the initial position of the aircraft is34.2∘N, 108.9∘E, altitude is 1500m, and the level flight speed is 400m/s. In SINS, the error characteristics of the three gyros and accelerometers are, respectively, consistent, the constant drift of the gyro is 0.01∘/h and the random walk is 0.001∘/h, the constant error of the accelerometer is 10−4g and the random walk is 10−5g⋅s, the initial alignment error is 0, the initial velocity error is 0.5m/s, and the initial position error is 15m. In SAR, the image matching calculation time is 2s, the antenna attitude angle error is 60″,60″,60″, distance measurement white noise is 40m, distance rate measurement white noise is 0.1m/s, horizontal positioning accuracy is 30m, the barometric altimeter error is 20m, and the simulation time is 1600s. The flight trajectory simulation of the aircraft is shown in Figure 2.Figure 2
Flight trajectory.This paper uses root mean squared errors (RMSE) of the estimation error to show the integrated navigation system filter performance of the comparison methods from a statistical perspective. The RMSE is defined as(25)RMSE=1M∑i=1Mx^i−xtrue,where M is the Monte Carlo runs. Take the estimated heading angle, velocity, and position error as an example, and the simulation results are shown in Figures 3–9, obtained by the proposed IAPF, APF, and PF algorithms.Figure 3
The heading angle estimation errors.Figure 4
The east velocity estimation errors.Figure 5
The north velocity estimation errors.Figure 6
The up velocity estimation errors.Figure 7
The longitude position estimation errors.Figure 8
The latitude position estimation errors.Figure 9
The height position estimation errors.It can be seen from the heading angle estimation errors simulation figures that the three-filter algorithms error estimation curve fluctuates significantly at the beginning of the simulation. After stabilization, the error estimation curve converges best by the IAPF algorithm and the RMSE of heading angle error is 6.218. Then, the RMSE by APF is 8.512, and the RMSE by PF is 13.570.It can be seen from the three directions of velocity error simulation figures and Table1 that the initial velocity errors of the three filters are relatively large, and the error curve basically tends to be stable after 100s. The RMSE of velocity error obtained by the proposed IAPF is the smallest, and the value is 0.288, 0.218, and 0.307, respectively. APF is second, and the RMSE of velocity error value is 0.347, 0.328, and 0.357, respectively. The east velocity error curve is the most obvious fluctuation; the RMSE of velocity error value is 0.686, 0.461, and 0.375, respectively.Table 1
Velocity error comparison for the simulation case (m/s).
Filter methodsRMSE ofδVeRMSE ofδVnRMSE ofδVuVelocity error rangePF0.6860.4610.375−1.12,1.83, −1.11,1.21, −0.82,0.80APF0.3470.3280.357−0.83,0.92, −0.31,0.90, −0.75,0.82IAPF0.2880.2180.307−0.30,0.71, −0.37,0.41, −0.35,0.52It can be seen from the three directions of position error simulation figures and Table2 that the RMSE of the position error obtained by IAPF is 3.397, 4.918, and 3.829, respectively. The RMSE of APF position error is 4.185, 7.063, and 4.663, respectively, and the RMSE of PF position error is 6.044, 8.219, and 6.388, respectively. These simulation results show that the IAPF can reduce location errors and improve the resolution accuracy for the SINS/SAR integrated system.Table 2
Position error comparison for the simulation case (m).
Filter methodsRMSE ofδLRMSE ofδλRMSE ofδHPosition error rangePF6.0448.2196.388−20.1,16.5, −21.0,25.0, −17.1,20.0APF4.1857.0634.663−11.2,12.0, −22.1,17.5, −8.1,10.2IAPF3.3974.9183.829−9.2,10.0, −9.5,17.5, −7.5,10.9


## 4.1. SINS/SAR Integrated Navigation System

The simulation experiment has been conducted to comprehensively evaluate and analyze the performance of the proposed algorithm for SINS/SAR integrated navigation system, and the comparison with the other two existing filters APF and PF is also discussed in this section. The SINS/SAR integrated navigation system makes use of the complementary characteristics of the SINS and SAR to improve the performance of the whole system [23, 24]. The schematic diagram of the SINS/SAR integrated navigation system is as in Figure 1.Figure 1
The schematic diagram of the SINS/SAR integrated navigation system.The local navigation frame (n-frame) is selected as the east-north-up (e-n-u) geography frame. Denote the inertial frame by i, the Earth frame by e, and the body frame by b. The dynamic model of the SINS/SAR integration navigation system can be expressed as(18)x˙=fx,t+wt,zt=hxt+vt,where fx,t is the system nonlinear state function, wt is the system process noise vector, h is the measurement matrix, vt is the measurement of white noise, and the mean value is zero. We define the system state vector as(19)xt=ϕe,ϕn,ϕu,δve,δvn,δvu,δL,δλ,δh,εxb,εyb,εzb,∇xb,∇yb,∇zbT,where ϕe,ϕn,andϕu denote attitude error quaternion of SINS, δve,δvn,andδvu denote velocity error in three directions of SINS, δL,δλ,andδh denote the errors of latitude, longitude, and height, εxb,εyb,andεzb denote random drift of the gyro, ∇xb,∇yb,and∇zb denote constant bias of the accelerometer, and wt is such as(20)wt=−Cω−1Cbn′ωabT,CbnωgbT,01×9T,whereCw−1,Cbn′, andCbn represent different rotation matrices. The attitude, velocity, and position error equations of SINS are given by [25, 26]. The system state equation in discrete-time form can be expressed as(21)xk=fxk−1+wk,where f⋅ is a discrete-time nonlinear function describing the dynamics of system state and wk is the discrete-time process noise [27].In this integrated system, the horizontal direction measurement is composed of the difference of heading angle, longitude, and latitude between SINS and SAR. The altitude direction measurement is composed of the difference of height between the SINS and the barometric altimeter. The system measurement equation in discrete-time form can be expressed as(22)zk=Hkxk+vk,and take the measurement vector as(23)zk=ψI−ψS,LI−LS,λI−λS,hI−heT,where ψI,LI,λI, and hI are, respectively, the heading angle and the position information for SINS; ψS,LS,andλS are the heading angle, latitude, and longitude position information for SAR. Due to the instability of the SINS system altitude measurement and the fact that the barometric altimeter is introduced to measure height information with he, δ denotes the corresponding error. zk can be rewritten as(24)zk=ψI−ψSLI−LSλI−λShI−he=ψ+δψ−ψ+δψSL+δL−L+δLSλ+δλ−λ+δλSh+δh−h+δhe=δψδLδλδh−δψSδLSδλSδhe.


## 4.2. Simulation Analysis

Assume the initial position of the aircraft is34.2∘N, 108.9∘E, altitude is 1500m, and the level flight speed is 400m/s. In SINS, the error characteristics of the three gyros and accelerometers are, respectively, consistent, the constant drift of the gyro is 0.01∘/h and the random walk is 0.001∘/h, the constant error of the accelerometer is 10−4g and the random walk is 10−5g⋅s, the initial alignment error is 0, the initial velocity error is 0.5m/s, and the initial position error is 15m. In SAR, the image matching calculation time is 2s, the antenna attitude angle error is 60″,60″,60″, distance measurement white noise is 40m, distance rate measurement white noise is 0.1m/s, horizontal positioning accuracy is 30m, the barometric altimeter error is 20m, and the simulation time is 1600s. The flight trajectory simulation of the aircraft is shown in Figure 2.Figure 2
Flight trajectory.This paper uses root mean squared errors (RMSE) of the estimation error to show the integrated navigation system filter performance of the comparison methods from a statistical perspective. The RMSE is defined as(25)RMSE=1M∑i=1Mx^i−xtrue,where M is the Monte Carlo runs. Take the estimated heading angle, velocity, and position error as an example, and the simulation results are shown in Figures 3–9, obtained by the proposed IAPF, APF, and PF algorithms.Figure 3
The heading angle estimation errors.Figure 4
The east velocity estimation errors.Figure 5
The north velocity estimation errors.Figure 6
The up velocity estimation errors.Figure 7
The longitude position estimation errors.Figure 8
The latitude position estimation errors.Figure 9
The height position estimation errors.It can be seen from the heading angle estimation errors simulation figures that the three-filter algorithms error estimation curve fluctuates significantly at the beginning of the simulation. After stabilization, the error estimation curve converges best by the IAPF algorithm and the RMSE of heading angle error is 6.218. Then, the RMSE by APF is 8.512, and the RMSE by PF is 13.570.It can be seen from the three directions of velocity error simulation figures and Table1 that the initial velocity errors of the three filters are relatively large, and the error curve basically tends to be stable after 100s. The RMSE of velocity error obtained by the proposed IAPF is the smallest, and the value is 0.288, 0.218, and 0.307, respectively. APF is second, and the RMSE of velocity error value is 0.347, 0.328, and 0.357, respectively. The east velocity error curve is the most obvious fluctuation; the RMSE of velocity error value is 0.686, 0.461, and 0.375, respectively.Table 1
Velocity error comparison for the simulation case (m/s).
Filter methodsRMSE ofδVeRMSE ofδVnRMSE ofδVuVelocity error rangePF0.6860.4610.375−1.12,1.83, −1.11,1.21, −0.82,0.80APF0.3470.3280.357−0.83,0.92, −0.31,0.90, −0.75,0.82IAPF0.2880.2180.307−0.30,0.71, −0.37,0.41, −0.35,0.52It can be seen from the three directions of position error simulation figures and Table2 that the RMSE of the position error obtained by IAPF is 3.397, 4.918, and 3.829, respectively. The RMSE of APF position error is 4.185, 7.063, and 4.663, respectively, and the RMSE of PF position error is 6.044, 8.219, and 6.388, respectively. These simulation results show that the IAPF can reduce location errors and improve the resolution accuracy for the SINS/SAR integrated system.Table 2
Position error comparison for the simulation case (m).
Filter methodsRMSE ofδLRMSE ofδλRMSE ofδHPosition error rangePF6.0448.2196.388−20.1,16.5, −21.0,25.0, −17.1,20.0APF4.1857.0634.663−11.2,12.0, −22.1,17.5, −8.1,10.2IAPF3.3974.9183.829−9.2,10.0, −9.5,17.5, −7.5,10.9


## 5. Conclusion

The proposed IAPF algorithm based on APF can obtain state estimation and covariance through adaptive regulation in the sampling process. The latest measurement information is efficiently utilized to establish a better importance density distribution function so that it is closer to the real distribution function. The process of weight adaptive adjustment and random weighted calculation can keep the diversity of particles and improve the filter precision. The simulations and comparison analysis have demonstrated that the proposed IAPF has preferable estimation accuracy than APF and PF in positioning calculation. It has been conducted to comprehensively verify the effectiveness and superiority of the proposed IAPF for SINS/SAR integrated navigation system, which provides a new method for the nonlinear model filter.Future work focuses on the improvement of the proposed algorithm to making the performance of the algorithm more perfect and how to reduce the complexity of calculation and apply this method to other integrated navigation systems.



---

*Source: 6635390-2021-01-31.xml*