ArticlePDF Available

Adaptive Square-Root Unscented Particle Filtering Algorithm for Dynamic Navigation

Authors:

Abstract and Figures

This paper presents a new adaptive square-root unscented particle filtering algorithm by combining the adaptive filtering and square-root filtering into the unscented particle filter to inhibit the disturbance of kinematic model noise and the instability of filtering data in the process of nonlinear filtering. To prevent particles from degeneracy, the proposed algorithm adaptively adjusts the adaptive factor, which is constructed from predicted residuals, to refrain from the disturbance of abnormal observation and the kinematic model noise. Cholesky factorization is also applied to suppress the negative definiteness of the covariance matrices of the predicted state vector and observation vector. Experiments and comparison analysis were conducted to comprehensively evaluate the performance of the proposed algorithm. The results demonstrate that the proposed algorithm exhibits a strong overall performance for integrated navigation systems.
Content may be subject to copyright.
sensors
Article
Adaptive Square-Root Unscented Particle Filtering
Algorithm for Dynamic Navigation
Wenhui Wei 1, *, Shesheng Gao 2, Yongmin Zhong 3ID , Chengfan Gu 4and Gaoge Hu 2
1School of Geological Engineering and Surveying and Mapping, Chang’An University, Xi’an 710064, China
2School of Automatics, Northwestern Polytechnical University, Xi’an 710072, China;
gshshnpu@nwpu.edu.cn (S.G.); hugaoge1111@126.com (G.H.)
3School of Engineering, RMIT University, Bundoora, Melbourne 3083, Australia;
yongmin.zhong@rmit.edu.au
4Department of Mechanical Engineering, University of Melbourne, Parkville, Melbourne 3010, Australia;
chengfan.gu@gmail.com
*Correspondence: weiwenhui91@chd.edu.cn; Tel.: +86-136-4926-6206
Received: 26 May 2018; Accepted: 17 July 2018; Published: 18 July 2018


Abstract:
This paper presents a new adaptive square-root unscented particle filtering algorithm
by combining the adaptive filtering and square-root filtering into the unscented particle filter to
inhibit the disturbance of kinematic model noise and the instability of filtering data in the process of
nonlinear filtering. To prevent particles from degeneracy, the proposed algorithm adaptively adjusts
the adaptive factor, which is constructed from predicted residuals, to refrain from the disturbance
of abnormal observation and the kinematic model noise. Cholesky factorization is also applied
to suppress the negative definiteness of the covariance matrices of the predicted state vector and
observation vector. Experiments and comparison analysis were conducted to comprehensively
evaluate the performance of the proposed algorithm. The results demonstrate that the proposed
algorithm exhibits a strong overall performance for integrated navigation systems.
Keywords:
performance analysis; particle filter; adaptive filtering; Cholesky factorization;
integrated navigation
1. Introduction
Nonlinear filtering is ubiquitous in many areas such as integrated navigation system, geodetic
positioning, automatic control, information fusion and signal processing. It aims to estimate the
state of a nonlinear dynamic system from observations. The extended Kalman filtering (EKF) is
a widely used filtering method for nonlinear systems [
1
,
2
]. It linearizes nonlinear system equations
by a truncated Taylor series expansion and then applies the linear Kalman filter to the linearized
system equations. However, it still requires the linearized state obey the Gaussian distribution,
which is usually not consistent with practical applications [
3
]. Further, when the probability function
of state distribution involves multiple peaks, the filtering solution will be biased or even divergent [
4
].
EKF also involves a complicated calculation process of solving Jacobian matrix. The unscented Kalman
filter (UKF) avoids the linearization error of EKF by approximating the probability density of state
distribution using unscented transformation (UT) [
5
,
6
]. It does not need to calculate Jacobian matrix.
However, this method inherits the linear update structure of the Kalman filtering and also requires
the system state obey the Gaussian distribution, which is unsuitable for nonlinear systems with
non-Gaussian system state model.
The particle filtering (PF) is an optimal recursive Bayesian filtering method based on Monte Carlo
simulation [
7
,
8
]. Since it is not limited by system linearity and the system state is not subject to the Gaussian
Sensors 2018,18, 2337; doi:10.3390/s18072337 www.mdpi.com/journal/sensors
Sensors 2018,18, 2337 2 of 15
distribution, this method can deal with nonlinear system models with non-Gaussian system state [
8
10
].
However, PF suffers from the particle degeneracy phenomenon and the accuracy largely depends on
the choice of importance sampling density function and resampling scheme [
11
13
]. Research efforts
have been focused on design of a good importance sampling density function and improvement of the
resampling scheme to improve the PF performance [
14
17
]. The unscented particle filtering (UPF) is
a method to obtain a better importance sampling density function using UT to approximate the posterior
probability density function of the state [
17
20
]. However, this method still suffers from the particle
degeneracy phenomenon if the dynamic system is affected by the disturbances of abnormal observation
and kinematic model noise [
10
,
17
,
20
]. In fact, the disturbances caused by abnormal observation or
kinematic model noise are unavoidable in practical engineering applications [
21
,
22
]. In addition, due to
the use of a large number of particles, PF also causes an expensive computational load. The parallel
implementation within a shared-memory architecture [
23
], reduced-order system modelling to reduce
the filtering dimensionality [
24
] and improvement of the algorithm structure can be used to improve the
computational performance of PF [18,20].
The robust adaptive filtering is a method to handle the problem of degradation or divergence
due to abnormal observation and kinematic model noise. It robustly estimates the covariance matrix
of observation noise and adaptively adjusts the covariance matrix of state noise by augmenting the
adaptive factor into the covariance matrix of state prediction to improve the filtering robustness [
21
,
22
,
25
].
Yang et al.
reported a robust adaptive filter by combining the robust maximum-likelihood estimation with
the adaptive filtering process to adaptively adjust the weight matrix of predicted parameters according
to the difference between system observation and model information [
26
]. This filter can be adaptively
converted into the classical Kalman filter, adaptive Kalman filter and Sage filter by modifying the weight
matrix and adaptive factor. Ding et al. reported a process noise scaling method by improving the
robustness of adaptive filtering, where the status of the filter operation is monitored using covariance
matching [
27
]. Gao et al. [
28
,
29
] combined the random weighting concept with adaptive filtering for
a dynamic navigation system. This method establishes unbiased random weighting estimations of
observation and state noises and feedbacks them to the kinematic and observation models of a dynamic
navigation system to improve the filtering robustness. Azam et al. [
30
,
31
] studied the online input
estimation techniques to handle cases in which the input of the robust adaptive filtering is unknown.
There are few studies focusing on the use of robust adaptive filtering to improve the
UPF performance.
Xue et al. [32]
reported a new robust adaptive unscented particle filtering algorithm.
In order to prevent particles from degeneracy, this algorithm adaptively determines the equivalent
weight function according to robust estimation and adaptively adjusts the adaptive factor constructed
from predicted residuals to inhibit the disturbances of abnormal observation and kinematic model noise.
However, due to the adaptive adjustment to the covariance matrices of predicted state vector and
observation vector, this algorithm cannot guarantee the covariance matrices in the filtering process are
positive definite, leading to the illness of the filtering process [
33
]. The square-root filtering provides
a solution to overcome this problem. It can improve the update accuracy of covariance matrices by
Cholesky factorization and effectively avoid the negative definiteness of covariance matrices.
This paper presents a new adaptive square-root unscented particle filtering (ASUPF) algorithm by
combining adaptive filtering and square-root filtering into UPF. This algorithm uses adaptive factors
to reasonably control the statistics of observation and kinematic models to inhibit the disturbances
of systematic noises, thus preventing particles from degeneracy. Further, Cholesky factorization is
used to suppress the negative definiteness of the covariance matrices of predicted state vector and
observation vector. Simulation and experimental analyses as well as comparison analysis with the
existing nonlinear filtering algorithms were conducted to comprehensively evaluate the performance
of the proposed nonlinear filtering algorithm for dynamic navigation.
Sensors 2018,18, 2337 3 of 15
2. Construction of Adaptive Factor
The role of the adaptive factor in the filtering process is to correct the predicted values using
the observation values, as well as to estimate and correct the unknown or inaccurate system model
parameters and noise statistics.
Consider the following nonlinear system
xk=f(xk1,vk1)
yk=h(xk,nk)(1)
where xkRnis the state vector at epoch k,ykRnis the system observation, vkRnis the process
noise with the variance
Rk
,
nkRn
is the observation noise with the variance
Qk
, both
f(·)
and
h(·)
are a nonlinear function and k=0, 1, · · · ,Nis the sampling epoch.
According to the theory of robust estimation, the predicted residual vector reflects the disturbance
of the dynamic system, since it contains the state information that has not been corrected by observation.
Therefore, the predicted residual vector can be used as the variable to construct the error discriminant
statistic and adaptive factor of the kinematic model. The predicted residual vector at time
k
can be
expressed as
Vk=ykyk(2)
where ykis the predicted observation vector.
Accordingly, the error discriminant statistic can be constructed by using Vk
Vk= VkTVk
tr(Pykyk)!1
2
(3)
where
Vk
is the error of the predicted residual vector,
Pykyk
is the covariance matrix of the predicted
observation vector and
tr(·)
represents the trace of a matrix. According to (3), three kinds of adaptive
factor can be constructed, namely the two-segment function adaptive factor, three-segment function
adaptive factor and exponential function adaptive factor [26].
The two-segment function adaptive factor can be constructed as
αk=(1Vkc
c/VkVk>c(4)
where αkrepresents the adaptive factor, satisfying 0 αk1 and c=1.0 2.5 is a constant.
The three-segment function adaptive factor can be constructed as
αk=
1Vkc0
c0
|Vk|c1|Vk|
c1c02
c0<Vkc1
0Vk>c1
(5)
where αksatisfies 0 αk1, c0=1.0 1.5 and c1=3.0 8.5 are constants.
The exponential function adaptive factor can be constructed as
αk=(1Vkc
e(|Vkc|)2Vk>c(6)
where αksatisfies 0 αk1, cis a constant and its value is usually 1.5.
Sensors 2018,18, 2337 4 of 15
3. Adaptive Square-Root Unscented Particle Filtering Algorithm
Abnormal interference can be caused by various system factors such as the additional thrust
change of carrier’s manoeuvre, mechanical disturbance, sensors anomaly and systematic noises and
various environmental factors such as air resistance, weather conditions and radiation. It will lead
to a sudden increase in observation error (i.e., the observation abnormality), or the inconformity
of the navigation kinematic model with the actual model (i.e., the model abnormality), leading to
a decrease in the accuracy of dynamic navigation. Combined with the advantages of adaptive filtering
and square-root filtering, an ASUPF algorithm for nonlinear systems is proposed in this section.
This algorithm selects appropriate adaptive factors to control the information of the kinematic
and observation models and suppresses the influence of abnormal interference, to improve the
filtering accuracy. Simultaneously, in order to suppress the negative definiteness of the covariance
matrices, Cholesky factorization is applied to the filtering process.
Consider the nonlinear system described as (1), the ASUPF algorithm includes the following steps.
Step 1: Initialization
Draw
N
sampling points according to the initial mean and variance. For
k=
0,
xi
0p(x0)
,
i=1, 2, · · · ,N. Assume
xi
0=E[xi
0]
Si
0=cholnE[(xi
0xi
0)(xi
0xi
0)T]o
wi
0=p(y0xi
0)
(7)
where
xi
0
and
xi
0
represent the
i
th initial particle and its estimated value,
Si
0
represents the
i
th Cholesky
factorization factor at the initial time,
wi
0
denotes the initial weight of the
i
th particle and
chol{·}
is the
Cholesky factorization operator.
Step 2: For k=1, 2, · · · ,N, conduct importance sampling.
(i)
Calculate the Sigma points and weights
xi
0,k1=xi
k1
xi
j,k1=xi
k1+p(N+λ)Si
k1j=1, · · · ,N
xi
j,k1=xi
k1p(N+λ)Si
k1j=n+1, · · · , 2N
(8)
Wm
0=λ/(N+λ)
Wc
0=λ/(N+λ) + (1α2+β)
Wm
j=Wc
j=0.5/(N+λ)j=1, · · · , 2 N
(9)
where
xi
j,k1
represents the
j
th Sigma point,
Wj
represents the weight of the
j
th Sigma point and
Wj=1
,
j=
0, 1,
· · ·
, 2
N
.
λ=α2(N+κ)
is the size factor,
κ
is the second-order size factor,
N
is
the number of particles,
α
is the factor determining the extent of sample distribution with respect
to the predicted state mean and 10
3<α
1.
β
is usually determined according to the prior
knowledge of the distribution of xand β=2 is optimal for the Gaussian distribution.
(ii)
Predict and update the particles using UKF
According to the kinematic model, the predicted state vector is expressed as
χi
j,k/k1=f(χi
j,k1)(10)
The estimate of the predicted state vector is calculated by
xi
k/k1=
2N
j=0
Wm
jχi
j,k/k1(11)
Sensors 2018,18, 2337 5 of 15
Applying Cholesky factorization to the covariance matrix of the predicted state vector yields
Si
k/k1=qrnhqWc
1(xi
1:2n,k/k1xi
k/k1)qPWkio (12)
Si
k/k1=cholupdatenSi
k/k1,xi
0,k/k1xi
k/k1,Wc
0o(13)
where chol update{·}represents the update operator of Cholesky factorization factor.
By using the adaptive factor αi
k,Si
k/k1can be modified
Si
k/k1=Si
k/k1/qαi
k(14)
where
αi
k
is constructed as (4) and the variance
Pi
ykyk
of observation information can be calculated by
Si
ykand Si
ˆ
yk.
According to the observation model, the observation vector can be written as
Yi
k|k1=h(χi
k|k1)(15)
The estimate of the observation vector is calculated as
yi
k/k1=
2N
j=0
Wc
jYi
j,k|k1(16)
Applying Cholesky factorization to the covariance matrix of the observation vector yields
Si
yk=qrqWc
1(Yi
1:2n,k/k1yi
k/k1)qPi
k1 (17)
Si
ˆ
yk=cholupdatenSi
ˆ
yk,Yi
0,k/k1yi
k/k1,Wc
0o(18)
where qr{·}represents the QR factorization of matrices.
The covariance matrix of χi
j,k|k1and Yi
j,k|k1can be obtained as
Pxkyk=
2N
j=0
Wc
j[(χi
j,k|k1xi
k|k1]·[Yi
j,k|k1yi
k|k1]T(19)
Update the state vector
xi
k=xi
k|k1+Ki
k(ykyi
k|k1). (20)
Update the covariance matrix of the estimated state vector
Ui=Ki
kSi
ˆ
yk(21)
Si
k=cholupdatenSi
k/k1,Ui,1o(22)
Pi
k=Si
k(Si
k)T(23)
where the gain matrix is expressed as
Ki
k= (Pi
xkyk/(Si
yk)T)/Si
yk(24)
It can be seen from above that the covariance matrix of the state vector is directly transmitted and
updated in the form of Cholesky factorization factor, thus ensuring the positive definiteness of the
covariance matrix and enhancing the numerical stability of the update process of the covariance matrix.
Sensors 2018,18, 2337 6 of 15
When the kinematic model is disturbed, the predicted residual increases and the adaptive factor
αi
k
decreases, leading to the reduced utilization of the predicted state. Accordingly, the interference of
model abnormality will be suppressed.
Let
N(xi
k
,
Pi
k)
calculated by (20) and (23) be the important density function and conduct importance
resampling to obtain the new particle xi
kN(xi
k,Pi
k).
Step 3: Calculate the weights
wi
k=wi
k1
p(ykxi
k)p(xi
kxi
k1)
q(xi
kxi
k1,yk)(25)
and normalize them as e
wi
k=wi
k/n
i=1
wi
k.
Step 4: Calculate the estimate threshold
ˆ
Ne f f =1/
N
i=1
(e
wi
k)2(26)
The severity of particle degeneracy can be determined by comparing the result obtained from (26)
with the established threshold. The smaller
ˆ
Ne f f
is, the worse the particles degeneracy is. In this case,
in order to inhibit particles degeneracy,
M
new particles can be resampled from the posterior density
function obtained above. Then, a common weight 1/ Mis assigned to each new particle.
Step 5: Calculate the estimate of the nonlinear state vector
ˆxk=
N
i=1e
wi
kxik(27)
Step 6: Go to Step 2 for the state estimation at the next epoch.
In the above recursive process of filtering, the proposed filter constantly checks whether there
is a change in the kinematic model. The original kinematic model will be modified according to
the change (if any) such that it can adapt to the dynamic change. In other words, the filter itself
constantly uses the noise statistical characteristic or gain matrix to reduce the estimated state error,
improve the filtering accuracy and provide a better sampling function for the importance sampling
process. Simultaneously, the Cholesky factorization of covariance matrices guarantees the stability of
the filtering process.
4. Performance Evaluation and Discussion
Experimental analysis was conducted to evaluate the performance of the proposed ASUPF.
The comparison analysis of ASUPF with EKF, UKF, PF and UPF was also conducted for the
performance evaluation.
4.1. Experimental Setup
An experiment was designed for ground vehicle navigation using a SINS/GPS integrated
navigation system. The experimental setup is shown in Figure 1. The test vehicle is a white urban
off-road vehicle, where an SINS/GPS integrated navigation system is mounted on the vehicle via
the fixed plate dynamic navigation. The vehicle also carries auxiliary facilities including a DC
power supply which is mounted on the vehicle via the fixed plate, an industrial personal computer
Sensors 2018,18, 2337 7 of 15
(IPC), a data memory and an ampere-voltage meter. Table 1provides the specifications of these
auxiliary facilities.
Sensors 2018, 18, x 7 of 15
Figure 1. Experimental setup.
Table 1. Specifications of the auxiliary facilities.
Item Model Specifications
IPC ADLINK
RK-610A
It is a 2.73 GHz Intel Core Duo CPU and 2.00 GB RAM PC, installed with GPS
Status Toolbox PRO v5.1. This PC is equipped with a navigation system
interface board and a 17-inch LCD monitor.
Data memory LCW-S02
It has a RS-232/485 interface, the storage rate is 10 KB/s and the storage
capacity is 32 G that can be expanded. The optional baud rate is 4800~115,200
bps. The file system is FAT32 and the storage file format is * .txt. The
operating temperature is 35 °C~85 °C.
DC power
supply
Sail
6-GFM-100
It consists of four groups of sustainable and stable discharge batteries, where
each battery rated voltage is 12 V and the rated capacity is
30.0 AH (10 h and the termination voltage of 10.8 V).
Ampere-voltage
meter
Transmit
G-2505
The voltage range is 0~50 V, the current range is 0~5 A and the measurement
accuracy is 0.5% FS.
Fixed plate It is a 10 mm thick steel plate with screw holes and bracket.
The framework of the experimental system is shown in Figure 2. The SINS/GPS integrated
navigation system provides SINS measurement, GPS positioning and integrated navigation results
(position, velocity and attitude), respectively. These navigation data are stored to the data memory
through the RS-232 interface and further transferred to IPC for post-processing and filtering. In
addition, the GPS Status Toolbox in IPC is adopted to dynamically monitor the environment for
GPS measurement, check the number and distribution of observable GPS constellations and control
the GPS initialization and operation. The monitoring data are fed back to IPC through the system
interface board. The ampere-voltage meter is used to dynamically measure the current and voltage
of the system interface to determine whether the SINS/GPS integrated navigation system works
normally or not.
Figure 1. Experimental setup.
Table 1. Specifications of the auxiliary facilities.
Item Model Specifications
IPC ADLINK RK-610A
It is a 2.73 GHz Intel Core Duo CPU and 2.00 GB RAM PC, installed with GPS
Status Toolbox PRO v5.1. This PC is equipped with a navigation system interface
board and a 17-inch LCD monitor.
Data memory LCW-S02
It has a RS-232/485 interface, the storage rate is 10 KB/s and the storage capacity is
32 G that can be expanded. The optional baud rate is 4800~115,200 bps. The file
system is FAT32 and the storage file format is * .txt. The operating temperature is
35 C~85 C.
DC power supply Sail 6-GFM-100
It consists of four groups of sustainable and stable discharge batteries, where each
battery rated voltage is 12 V and the rated capacity is 30.0 AH (10 h and the
termination voltage of 10.8 V).
Ampere-voltage meter Transmit G-2505 The voltage range is 0~50 V, the current range is 0~5 A and the measurement
accuracy is 0.5% FS.
Fixed plate It is a 10 mm thick steel plate with screw holes and bracket.
The framework of the experimental system is shown in Figure 2. The SINS/GPS integrated
navigation system provides SINS measurement, GPS positioning and integrated navigation results
(position, velocity and attitude), respectively. These navigation data are stored to the data memory
through the RS-232 interface and further transferred to IPC for post-processing and filtering.
In addition, the GPS Status Toolbox in IPC is adopted to dynamically monitor the environment
for GPS measurement, check the number and distribution of observable GPS constellations and control
the GPS initialization and operation. The monitoring data are fed back to IPC through the system
interface board. The ampere-voltage meter is used to dynamically measure the current and voltage of
the system interface to determine whether the SINS/GPS integrated navigation system works normally
or not.
Sensors 2018,18, 2337 8 of 15
Figure 2. The framework of the experimental system.
The parameters of the SINS/GPS integrated navigation system are provided in Table 2.
Table 2. The parameters of the SINS/GPS integrated navigation system.
Parameter Value
Update Rate SINS 125 Hz, GPS 5 Hz
Start Time <1 s
Operating Temperature 30 C~+60 C
Angular Velocity Measurement
Measuring Range ±200 /s
Zero-bias Stability 10.0 /h (1σ)
Scale Factor 0.1% (1 σ)
Non-linear 0.01% FS (1σ)
Random Walk Coefficient 1.0 /hr1/2 (1σ)
Acceleration Measurement
Measuring Range ±20 g
Zero-bias Stability 2 mg (1σ)
Scale Factor 0.1% (1σ)
Non-linear 0.01% FS (1σ)
Random Walk Coefficient 0.005 m/s/hr1/2 (1σ)
GPS Measurement
L1/L2 Horizontal Accuracy 1.0 m,
Vertical Accuracy 1.5 m (1σ)
SBAS Horizontal Accuracy 0.6 m,
Vertical Accuracy 1.0 m (1σ)
DGPS Horizontal Accuracy 0.3 m,
Vertical Accuracy 0.5 m (1σ)
Velocity Accuracy 0.02 m/s (1σ)
After the one-minute initialization of the SINS/GPS integrated system, the test vehicle started
to travel to the East along the Huanshan Road to the Fengyu Kou roundabout. The start position
of the vehicle was (E108
46
0
05.89”, N34
01
0
41.24”). When arriving at the Fengyu Kou roundabout,
the vehicle turned at the position (E108
49
0
04.61”, N34
03
0
10.28”) and then travelled back to the
start position. The travelling trajectory of the test vehicle and associated position coordinates are
shown in Figures 3and 4, respectively. The travelling distance was 12.38 km, the travelling time was
19 min and the average speed of the vehicle was 39.1 km/h. During the test process, the GPS receiver
received signals from at least seven navigation stars. The data obtained from the high-precision
differential GPS receiver C-Nav3050 were used as reference for the comparison with the positioning
results from the SINS/GPS integration system.
Sensors 2018,18, 2337 9 of 15
Sensors 2018, 18, x 9 of 15
Figure 3. Vehicle traveling trajectory.
108.76 108.77 108.78 108.79 108.8 108.81 108.82 108.83
34.025
34.03
34.035
34.04
34.045
34.05
34.055
34.06
Longitude/
°
Latitude/
°
The starting point
Travelling trajectory
The turning point
Figure 4. The position coordinates of the vehicle travelling trajectory.
4.2. System Models of SINS/GPS Integrated Navigation
The navigation frame is the E-N-U (East-North-Up) geographic coordinate system. The state
vector
()tx
of the SINS/GPS integrated navigation system is defined as
T
() , , , , , , , , , , , , , ,
ENU E N U x yz x y z
tvvvLh
φφφδ δ δ δ δλδ εεε

=∇

x (28)
where (, , )
E
NU
φφφ
is the attitude error, (, , )
E
NU
VVV
δδδ
is the velocity error, (,,)
L
h
δδλδ
is
the position error in latitude, longitude and altitude, (, , )
x
yx
εεε
represents the random drift of
the gyroscope and (, , )
x
yz
∇∇∇ is the constant bias of the accelerometer.
The kinematic model of the SINS/GPS integrated navigation system is expressed as
() ( ,) () ()tft tt=+xxGw
(29)
where (,)
f
tx is the nonlinear state function of the system, T
() [ , , , , , ]
gx gy gz ax ay az
twwwwww=w
is the system noise consisting of gyro’s Gaussian white noise (,,)
g
xgygz
www and accelerometer’s
Gaussian white noise (, , )
ax ay az
www and ()tG is the coefficient matrix of the system noise.
Figure 3. Vehicle traveling trajectory.
Sensors 2018, 18, x 9 of 15
Figure 3. Vehicle traveling trajectory.
108.76 108.77 108.78 108.79 108.8 108.81 108.82 108.83
34.025
34.03
34.035
34.04
34.045
34.05
34.055
34.06
Longitude/
°
Latitude/
°
The starting point
Travelling trajectory
The turning point
Figure 4. The position coordinates of the vehicle travelling trajectory.
4.2. System Models of SINS/GPS Integrated Navigation
The navigation frame is the E-N-U (East-North-Up) geographic coordinate system. The state
vector
()tx
of the SINS/GPS integrated navigation system is defined as
T
() , , , , , , , , , , , , , ,
ENU E N U x yz x y z
tvvvLh
φφφδ δ δ δ δλδ εεε

=∇

x (28)
where (, , )
E
NU
φφφ
is the attitude error, (, , )
E
NU
VVV
δδδ
is the velocity error, (,,)
L
h
δδλδ
is
the position error in latitude, longitude and altitude, (, , )
x
yx
εεε
represents the random drift of
the gyroscope and (, , )
x
yz
∇∇∇ is the constant bias of the accelerometer.
The kinematic model of the SINS/GPS integrated navigation system is expressed as
() ( ,) () ()tft tt=+xxGw
(29)
where (,)
f
tx is the nonlinear state function of the system, T
() [ , , , , , ]
gx gy gz ax ay az
twwwwww=w
is the system noise consisting of gyro’s Gaussian white noise (,,)
g
xgygz
www and accelerometer’s
Gaussian white noise (, , )
ax ay az
www and ()tG is the coefficient matrix of the system noise.
Figure 4. The position coordinates of the vehicle travelling trajectory.
4.2. System Models of SINS/GPS Integrated Navigation
The navigation frame is the E-N-U (East-North-Up) geographic coordinate system. The state
vector x(t)of the SINS/GPS integrated navigation system is defined as
x(t) = φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εx,εy,εz,x,y,zT(28)
where
(φE
,
φN
,
φU)
is the attitude error,
(δVE
,
δVN
,
δVU)
is the velocity error,
(δL
,
δλ
,
δh)
is the position
error in latitude, longitude and altitude,
(εx
,
εy
,
εx)
represents the random drift of the gyroscope and
(x,y,z)is the constant bias of the accelerometer.
The kinematic model of the SINS/GPS integrated navigation system is expressed as
.
x(t)=f(x,t) + G(t)w(t)(29)
Sensors 2018,18, 2337 10 of 15
where
f(x
,
t)
is the nonlinear state function of the system,
w(t) = [wgx,wgy,wgz ,wax,way,waz ]T
is the
system noise consisting of gyro’s Gaussian white noise
(wgx
,
wgy
,
wgz)
and accelerometer’s Gaussian
white noise wax,way,waz )and G(t)is the coefficient matrix of the system noise.
The observation model is described as
zk=δρk=Hkxk+1
2
xT
k·DT
v·Ce
gT·H(1)(rins )·Ce
g·Dv·xk
xT
k·DT
v·Ce
gT·H(2)(rins )·Ce
g·Dv·xk
xT
k·DT
v·Ce
gT·H(3)(rins )·Ce
g·Dv·xk
xT
k·DT
v·Ce
gT·H(4)(rins )·Ce
g·Dv·xk
+vk(30)
where
δρk
is the pseudo-range difference of GPS satellites,
Hk
is the observation matrix,
vk
is the
observation noise,
Ce
g
is the transformation matrix from the geographic coordinate system to the earth
coordinate system,
rins
is the INS position vector and
Dv
is an auxiliary matrix, which is expressed as
Dv=h03×6I3×303×8i(31)
4.3. Filtering Accuracy
For comparison analysis, trials based on the above experimental design were conducted by using
EKF, UKF, PF, UPF and ASUPF, respectively. The unscented transformation parameters were
α=
0.5
and
β=
2. The adaptive factor calculation parameters were
c0=
1 and
c1=
3.5. The sampling time
was 1000 s. 50 Monte Carlo simulations were conducted for each of the five filters.
Since the position errors in the other directions have the similar trends as that in the longitude
direction, only the position error in the longitude direction is discussed for conciseness. Figure 5shows
the longitude errors of EKF and UKF. It can be seen that EKF has the poor filtering accuracy, due to the
error caused by the linearization of the nonlinear state model. Although UKF improves the filtering
accuracy of EKF, the improved accuracy is still limited. This is because UKF approximates the posterior
probability distribution of the system state using the Gaussian distribution. Its filtering accuracy is
significantly degraded when the posterior probability distribution of the system state is non-Gaussian
distribution, which is the case of the experimental test. Therefore, both EKF and UKF have limited
accuracy for strongly nonlinear systems.
Sensors 2018, 18, x 10 of 15
The observation model is described as
()
()
()
()
()
()
()
()
TTT(1)
TTT(2)
TTT(3)
TTT(4)
1
2
ee
kvg insgvk
ee
kvg insgvk
kkkk k
ee
kvg insgvk
ee
kvg insgvk
δ

⋅⋅⋅ ⋅


⋅⋅⋅ ⋅

== +

⋅⋅⋅ ⋅


⋅⋅⋅ ⋅

xC rCx
xC rCx
z
x+v
xC rCx
xC rCx
DH D
DH D
H
DH D
DH D
ρ
(30)
where k
δ
ρ
is the pseudo-range difference of GPS satellites, k
H is the observation matrix, k
v is
the observation noise, e
g
C is the transformation matrix from the geographic coordinate system to
the earth coordinate system, ins
r is the INS position vector and v
D is an auxiliary matrix, which
is expressed as
[
]
36 33 38v×××
=D0 I 0
(31)
4.3. Filtering Accuracy
For comparison analysis, trials based on the above experimental design were conducted by
using EKF, UKF, PF, UPF and ASUPF, respectively. The unscented transformation parameters were
0.5
α
= and 2
β
=. The adaptive factor calculation parameters were 01c= and 13.5c=. The
sampling time was 1000 s. 50 Monte Carlo simulations were conducted for each of the five filters.
Since the position errors in the other directions have the similar trends as that in the longitude
direction, only the position error in the longitude direction is discussed for conciseness. Figure 5
shows the longitude errors of EKF and UKF. It can be seen that EKF has the poor filtering accuracy,
due to the error caused by the linearization of the nonlinear state model. Although UKF improves
the filtering accuracy of EKF, the improved accuracy is still limited. This is because UKF
approximates the posterior probability distribution of the system state using the Gaussian
distribution. Its filtering accuracy is significantly degraded when the posterior probability
distribution of the system state is non-Gaussian distribution, which is the case of the experimental
test. Therefore, both EKF and UKF have limited accuracy for strongly nonlinear systems.
0100 200 300 400 500 600 700 800 900 1000
-10
-5
0
5
10
t(s)
The longitude direction position error(m)
EKF
UKF
Figure 5. The longitude errors of EKF and UKF.
Figure 5. The longitude errors of EKF and UKF.
Figure 6shows the longitude errors of PF, UPF and ASUPF, where the particle number is
M=
200.
Compared to Figure 5, it is obvious that all three particle filters (PF, UPF and ASUPF) have higher
accuracy than both EKF and UKF. This is mainly because these three particle filters describe the priori
Sensors 2018,18, 2337 11 of 15
and posteriori information using samples instead of a function, thus overcoming the limitation of
both EKF and UKF that random variables must satisfy the Gaussian distribution. However, PF suffers
from the particle degradation phenomenon, leading to the limited filtering accuracy. UPF improves
the filtering accuracy of PF, as it generates the importance function and conducts resampling using
UT to weaken the phenomenon of particle degradation. However, due to the influence of abnormal
interference on the state estimation, the filtering curve of UPF still involves large oscillations. As clearly
shown in Figure 6, the abnormal interference caused by the sharp U-turn travelling at around
t=500 s
significantly affects the performances of PF and UPF. In contrast, ASUPF improves UPF by
introducing the adaptive factor to suppress the influence of abnormal interference on the kinematic
and observation models. Therefore, ASUPF has much higher accuracy than both PF and UPF. Table 3
lists the root mean square errors (RMSEs) in the longitude direction for each nonlinear filter.
Sensors 2018, 18, x 11 of 15
Figure 6 shows the longitude errors of PF, UPF and ASUPF, where the particle number is
200M=. Compared to Figure 5, it is obvious that all three particle filters (PF, UPF and ASUPF)
have higher accuracy than both EKF and UKF. This is mainly because these three particle filters
describe the priori and posteriori information using samples instead of a function, thus overcoming
the limitation of both EKF and UKF that random variables must satisfy the Gaussian distribution.
However, PF suffers from the particle degradation phenomenon, leading to the limited filtering
accuracy. UPF improves the filtering accuracy of PF, as it generates the importance function and
conducts resampling using UT to weaken the phenomenon of particle degradation. However, due
to the influence of abnormal interference on the state estimation, the filtering curve of UPF still
involves large oscillations. As clearly shown in Figure 6, the abnormal interference caused by the
sharp U-turn travelling at around 500t=s significantly affects the performances of PF and UPF. In
contrast, ASUPF improves UPF by introducing the adaptive factor to suppress the influence of
abnormal interference on the kinematic and observation models. Therefore, ASUPF has much
higher accuracy than both PF and UPF. Table 3 lists the root mean square errors (RMSEs) in the
longitude direction for each nonlinear filter.
0100 200 300 400 500 600 700 800 900 1000
-10
-5
0
5
10
The longitude direct ion position error(m)
t(s)
PF
UPF
ASUPF
Figure 6. The longitude errors of PF, UPF and ASUPF ( 200M=).
Table 3. The mean values of the longitude RMSEs for EKF, UKF, UPF and ASUPF.
Filter Mean Value of Longitude RMSEs/m Normalized Mean Value
EKF 3.62 0.7240
UKF 2.56 0.5120
PF ( 200M=) 2.13 0.4260
UPF ( 200M=) 1.15 0.2300
ASUPF ( 200M=) 0.46 0.0920
Figure 7 shows the means of the longitude RMSEs for the five filters, where the means of the
RMSEs of the three particle filters (PF, UPF and RAUPF) are subject to three different particle
numbers 50M=, 200M= and 500M=. It can be seen that both EKF and UKF involves a large
error. However, all three particle filters still have higher accuracy than both EKF and UKF, even
with the small number of particles ( 50M=).
Figure 6. The longitude errors of PF, UPF and ASUPF (M=200).
Table 3. The mean values of the longitude RMSEs for EKF, UKF, UPF and ASUPF.
Filter Mean Value of Longitude RMSEs/m Normalized Mean Value
EKF 3.62 0.7240
UKF 2.56 0.5120
PF (M=200 ) 2.13 0.4260
UPF (M=200 ) 1.15 0.2300
ASUPF (M=200 ) 0.46 0.0920
Figure 7shows the means of the longitude RMSEs for the five filters, where the means of
the RMSEs of the three particle filters (PF, UPF and RAUPF) are subject to three different particle
numbers
M=
50,
M=
200 and
M=
500. It can be seen that both EKF and UKF involves
a large error.
However, all three particle
filters still have higher accuracy than both EKF and UKF,
even with the small number of particles (M=50).
Sensors 2018,18, 2337 12 of 15
Sensors 2018, 18, x 12 of 15
12345
0
0.5
1
1.5
2
2.5
3
3.5
4
Mean values of longitude RMSEs (m)
M=50
1 2 3 4 5
0
0.5
1
1.5
2
2.5
3
3.5
4
M=200
1 2 3 4 5
0
0.5
1
1.5
2
2.5
3
3.5
4
M=500
Figure 7. Mean values of the longitude RMSEs of EKF, UKF, PF, UPF and ASUPF, where the mean
values for PF, UPF and ASUPF are subject to different particle numbers 50M=, 200M= and
500M= and the numbers from 1 to 5 indicate EKF, UKF, PF, UPF and ASUPF, respectively.
4.4. Computational Performance and Filtering Robustness
Trials were conducted with Matlab programs on a 2.93 GHz dual-core CPU and 2 G RAM PC
to analyse the computational performances of EKF, UKF, PF, UPF and ASUPF, where the particle
number was set to 200M= for PF, UPF and ASUPF. Table 4 shows the computational
performances of each filter.
Table 4. Computational performances of EKF, UKF, PF, UPF and ASUPF.
Filter Equivalent Computational
Complexity
Peak of CPU
Utilization
Running
Time/s
Normalized
Running Time/s
EKF 3
()On 18% 0.202 0.0505
UKF 4
()On 23% 0.958 0.2395
PF 3
()OMn 42% 2.411 0.6028
UPF 34
()OMn n+ 48% 3.078 0.7695
ASUPF 34
()OMn n+ 49% 3.089 0.7722
It can be seen that the computational times of PF, UPF and ASUPF are obviously larger than
those of EKF and UKF. This is because the computational processes of these three particle filters are
more complex, involving sampling a large number of particles, allocating weights and resampling.
Thus, they require more CPU utilizations.
In order to analyse the robust performances of EKF, UKF, PF, UPF and ASUPF, the above
experimental data were divided into two groups. One was within the sharp U-turn time period
(484.2 s, 512.8 s) and the other was within the rest time periods. Based on each group of
experimental data, the longitude RMSEs of EKF, UKF, PF, UPF and ASUPF were calculated, where
the particle number was set to 200M= for PF, UPF and ASUPF. The RMSE differences between
the two groups of experimental data indicate the robust performances of each filter. Table 5 shows
the results on the robustness of each filter.
Figure 7.
Mean values of the longitude RMSEs of EKF, UKF, PF, UPF and ASUPF, where the mean
values for PF, UPF and ASUPF are subject to different particle numbers
M=
50,
M=
200 and
M=
500
and the numbers from 1 to 5 indicate EKF, UKF, PF, UPF and ASUPF, respectively.
4.4. Computational Performance and Filtering Robustness
Trials were conducted with Matlab programs on a 2.93 GHz dual-core CPU and 2 G RAM PC
to analyse the computational performances of EKF, UKF, PF, UPF and ASUPF, where the particle
number was set to
M=
200 for PF, UPF and ASUPF. Table 4shows the computational performances of
each filter.
Table 4. Computational performances of EKF, UKF, PF, UPF and ASUPF.
Filter Equivalent
Computational Complexity
Peak of
CPU Utilization Running Time/s Normalized
Running Time/s
EKF O(n3)18% 0.202 0.0505
UKF O(n4)23% 0.958 0.2395
PF O(Mn3)42% 2.411 0.6028
UPF O(Mn3+n4)48% 3.078 0.7695
ASUPF O(Mn3+n4)49% 3.089 0.7722
It can be seen that the computational times of PF, UPF and ASUPF are obviously larger than
those of EKF and UKF. This is because the computational processes of these three particle filters are
more complex, involving sampling a large number of particles, allocating weights and resampling.
Thus, they require more CPU utilizations.
In order to analyse the robust performances of EKF, UKF, PF, UPF and ASUPF, the above
experimental data were divided into two groups. One was within the sharp U-turn time period
(484.2 s, 512.8 s) and the other was within the rest time periods. Based on each group of experimental
data, the longitude RMSEs of EKF, UKF, PF, UPF and ASUPF were calculated, where the particle
number was set to
M=
200 for PF, UPF and ASUPF. The RMSE differences between the two groups
of experimental data indicate the robust performances of each filter. Table 5shows the results on the
robustness of each filter.
Sensors 2018,18, 2337 13 of 15
Table 5. Robust performances of EKF, UKF, PF, UPF and ASUPF.
Filter Longitude Direction Position RMSE/m Normalized
Difference
The Sharp U-turn Time Period The Other Time Periods Difference
EKF 5.3584 3.5753 1.7831 0.8915
UKF 4.1364 2.8243 1.3121 0.6561
PF 3.1658 2.0370 1.1288 0.5644
UPF 1.5469 0.8663 0.6806 0.3403
ASUPF 0.5517 0.4191 0.1326 0.0663
It can be seen from Table 5that abnormal disturbances affect EKF, UKF and PF more significantly
than UPF and ASUPF. This is also in agreement with the oscillations in the error curves of EKF, UKF
and PF as shown in Figures 5and 6. However, the influence of abnormal disturbances on ASUPF is
even more than twice smaller than that on UPF. This is because ASUPF can control the noise statistics
of the kinematic and observation models by adjusting the adaptive factor to suppress the influence of
abnormal interferences.
4.5. Overall Performance
Define the overall performance index of a filtering algorithm as
S=f(p,RT,R) = 1
W·(p,RT,R)T(32)
where
S
represents the overall performance index of the filtering algorithm and the larger the value
is, the better the performance of the algorithm is
p
,
RT
and
R
are the three performances of the
filtering algorithm, that is, the accuracy, computational performance and robustness, respectively.
W= (β1,β2,β3), where βi(i=1, 2, 3) are the weights of the three performances, respectively, and
3
i=1
βi=10βi1 (33)
Under different performance requirements of a navigation system, the value of
W= (β1
,
β2
,
β3)
is different. For example,
W= (
0.6, 0.2, 0.2
)
indicates that the priority of the navigation system is
the positioning accuracy, while the computational performance and robustness are subservient to the
positioning accuracy.
Table 6shows the overall performance indices of EKF, UKF, UPF and ASUPF under three different
priorities of accuracy, computational performance and robustness (represented by the three values
of
W
), where the values of
p
,
RT
and
R
correspond to the normalized values of the three performances
as shown in Tables 35, respectively.
Table 6. Overall performance indexes of the nonlinear filtering algorithms.
Filtering Algorithms Accuracy PriorityW = (0.6,0.2,0.2)Timing PriorityW = (0.2,0.6,0.2)Robustness PriorityW = (0.2,0.2,0.6)
EKF 1.6056 2.8296 1.4496
UKF 2.0563 2.6503 1.8385
PF 2.0449 1.7866 1.8369
UPF 2.7781 1.7368 2.4748
ASUPF 4.4861 2.0202 4.7030
The overall performance indices of EKF and UKF under the three different priorities show that
both EKF and UKF have a strong advantage in the computational performance. Although both
accuracy and robustness are weak, the accuracy performance is better than the robustness performance
for both EKF and UKF. This is also in agreement with the experimental results of EKF and UKF
(see Figures 5and 7and Table 5).
Sensors 2018,18, 2337 14 of 15
The overall performance indices of ASUPF under the three different priorities show that ASUPF
has strong accuracy and robustness performances and its robustness is highest in Table 6. This proves
that the improvement of ASUPF in adaptability and stability is effective. Although the computational
performance of ASUPF is lower than those of EKF and UKF, ASUPF has a much better overall
performance than the other filters for the integrated navigation system.
In general, the performance requirements of a navigation system determine the selection of
an appropriate filter. For a navigation system desiring high accuracy and strong robustness, ASUPF
should be considered. For a navigation system desiring a high computational performance, either EKF
or UKF should be considered.
5. Conclusions
This paper presents a new ASUPF for nonlinear systems by combining adaptive filtering and
square-root filtering into UPF. This algorithm improves UPF by using the adaptive factor to refrain
from the disturbances of the noise statistics of observation and kinematic models, thus overcoming
the particle degeneracy problem involved in UPF. It also applies Cholesky factorization to suppress
the negative definiteness of the covariance matrices of predicted state vector and observation vector.
Experiments and comparison analysis demonstrate that the proposed ASUPF can effectively prevent
particles from degeneracy and improve the filtering accuracy of dynamic navigation. Future work will
focus on the sensitivity analysis of the proposed ASUPF in comparison with the existing nonlinear
filtering algorithms such as EKF, UKF, PF and UPF.
Author Contributions:
W.W. completed the theoretical research and design of the manuscript, including the
principle and design of the filtering algorithm and completed the writing of the manuscript. S.G. guided and
supervised the research content and technical principle of the algorithms in the manuscript. Y.Z. conducted the
experimental analysis in the manuscript, including data collection and charts drawing. C.G. provided technical
inputs, reviewed the experimental scheme and contributed to the writing of the manuscript. Research associate
G.H. completed the literature retrieval and guided the experimental process.
Funding:
The work of this paper was supported by the National Natural Science Foundation of China (Project
No. 41704016), the China Postdoctoral Science Foundation (Project No. 2017M613029) and the Postdoctoral
Research Project Foundation of Shan’xi Province (Project No. 2017BSHEDZZ84).
Conflicts of Interest: The authors declare no conflict of interest.
References
1.
Li, W.; Jia, Y.; Du, J. Distributed extended Kalman filter with nonlinear consensus estimate. J. Frankl. Inst.
2017,354, 7983–7995. [CrossRef]
2.
Li, J.; Wei, X.; Zhang, G. An Extended Kalman Filter-Based Attitude Tracking Algorithm for Star Sensors.
Sensors 2017,17, 1921.
3.
Simon, D. Optimal state estimation. In Infinity and Nonlinear Approaches; Kalman, H., Ed.; John Wiley & Sons:
Hoboken, NJ, USA, 2006.
4. Hu, G.; Gao, S.; Zhong, Y. A derivative UKF for tightly coupled INS/GPS integrated navigation. ISA Trans.
2015,56, 135–144. [CrossRef] [PubMed]
5.
Liu, X.; Qu, H.; Zhao, J.; Yue, P.; Wang, M. Maximum Correntropy Unscented Kalman Filter for Spacecraft
Relative State Estimation. Sensors 2016,16, 1530. [CrossRef] [PubMed]
6.
Gao, S.; Hu, G.; Zhong, Y. Windowing and random weighting-based adaptive unscented Kalman filter.
Int. J. Adapt. Control Signal Process. 2015,29, 201–223. [CrossRef]
7.
Doucet, A.; Godsill, S.; Andrieu, C. On sequential Monte Carlo methods for Bayesian filtering. Stat. Comput.
2000,10, 197–208. [CrossRef]
8.
Rawlings, J.B.; Bakshi, B.R. Particle filtering and moving horizon estimation. Comput. Chem. Eng.
2006
,30,
1529–1541. [CrossRef]
9.
Zhang, B.; Chen, M.; Zhou, D.; Li, Z. Particle-filter-based estimation and prediction of chaotic states.
Chaos Solitons Fractals 2007,32, 1491–1498. [CrossRef]
Sensors 2018,18, 2337 15 of 15
10.
Oppenheim, G.; Philippe, A.; de Rigal, J. The particle filters and their applications. Chemom. Intell. Lab. Syst.
2008,91, 87–93. [CrossRef]
11.
Ming, Q.; Jo, K.H. A novel particle filter implementation for a multiple-vehicle detection and tracking system
using tail light segmentation. Int. J. Control Autom. Syst. 2013,11, 577–585.
12.
Arulampalam, M.S.; Maskell, S.; Gordon, N.; Clapp, T. A tutorial on particle filters for online
nonlinear/non-Gaussian Bayesian tracking. IEEE Trans. Signal Process. 2002,50, 174–188. [CrossRef]
13.
Li, Y.; Sun, S.; Hao, G. A Weighted Measurement Fusion Particle Filter for Nonlinear Multisensory Systems
Based on Gauss-Hermite Approximation. Sensors 2017,17, 2222. [CrossRef] [PubMed]
14.
Wang, X.; Ni, W. An improved particle filter and its application to an INS/GPS integrated navigation system
in a serious noisy scenario. Meas. Sci. Technol. 2016,27, 095005. [CrossRef]
15.
Rabbou, M.A.; El-Rabbany, A. Integration of GPS precise point positioning and MEMS-based INS using
unscented particle filter. Sensors 2015,15, 7228–7245. [CrossRef] [PubMed]
16.
Budhiraja, A.; Chen, L.; Lee, C. A survey of numerical methods for nonlinear filtering problems. Phys. D
Nonlinear Phenom. 2007,230, 27–36. [CrossRef]
17.
Tao, L.; Yuan, G.; Wang, L. Particle Filter with Novel Nonlinear Error Model for Miniature Gyroscope-Based
Measurement While Drilling Navigation. Sensors 2016,16, 371.
18.
Van der Merwe, R.; Doucet, A.; Freitas, N.; Wan, E. The Unscented Particle Filter; Tech. Rep. CUED/F-INFENG/TR 380;
Engineering Department, Cambridge University: London, UK, 2000.
19.
Liang, Z.; Ma, X.; Dai, X. Robust tracking of moving sound source using scaled unscented particle filter.
Appl. Acoust. 2008,69, 673–680. [CrossRef]
20.
Ali, J.; Fang, J. Realization of an autonomous integrated suite of strapdown astro-inertial navigation systems
using unscented particle filtering. Comput. Math. Appl. 2009,57, 169–183. [CrossRef]
21.
Yang, Y.; Cui, X. Adaptively robust filter with multi adaptive factors. Surv. Rev.
2008
,40, 260–270. [CrossRef]
22.
Gao, S.; Zhong, Y.; Li, W. Robust adaptive filtering method for SINS/SAR integrated navigation system.
Aerosp. Sci. Technol. 2011,15, 425–430. [CrossRef]
23.
Azam, S.E.; Ghisi, A.; Mariani, S. Parallelized sigma-point Kalman filtering for structural dynamics.
Comput. Struct. 2012,92, 193–205. [CrossRef]
24.
Azam, S.E.; Mariani, S. Online damage detection in structural systems via dynamic inverse analysis:
A recursive Bayesian approach. Eng. Struct. 2018,159, 28–45. [CrossRef]
25.
Sayed, H.; Rupp, M. Robust issues in adaptive filtering. In Digital Signal Processing Fundamentals;
Madisetti, V.K., Ed.; Taylor & Francis: Boca Raton, FL, USA, 2010.
26.
Yang, Y.; He, H.; Xu, G. A new adaptive robust filtering for kinematic geodetic positioning. J. Geodesy
2001
,
75, 109–116. [CrossRef]
27.
Ding, W.; Wang, J.; Rizos, C. Improving adaptive Kalman estimation in GPS/INS integration. J. Navig.
2007
,
60, 517–529. [CrossRef]
28.
Gao, S.; Gao, Y.; Zhong, Y.; Wei, W. Random weighting estimation method for dynamic navigation positioning.
Chin. J. Aeronaut. 2011,24, 318–323. [CrossRef]
29.
Gao, S.; Wei, W.; Zhong, Y.; Subic, A. Sage Windowing and Random Weighting Adaptive Filtering Method
for Kinematic Model Error. IEEE Trans. Aerosp. Electron. Syst. 2015,51, 1488–1500. [CrossRef]
30.
Azam, S.E.; Chatzi, E.; Papadimitriou, C. A dual Kalman filter approach for state estimation via output-only
acceleration measurements. Mech. Syst. Signal Process. 2015,60, 866–886. [CrossRef]
31.
Azam, S.E.; Chatzi, E.; Papadimitriou, C.; Smyth, A. Experimental validation of the Kalman-type filters for
online and real-time state and input estimation. J. Vib. Control 2017,23, 2494–2519. [CrossRef]
32.
Xue, L.; Gao, S.; Zhong, Y. Robust adaptive unscented particle filter. Int. J. Intell. Mechatron. Robot.
2013
,3,
55–56. [CrossRef]
33.
Zhang, N.; Yang, X. Gaussian Mixture Unscented Particle Filter with Adaptive Residual Resample
for Nonlinear Model. In Proceedings of the International Conference on Intelligent Computing and
Cognitive Informatics, Singapore, 8–9 September 2015.
©
2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access
article distributed under the terms and conditions of the Creative Commons Attribution
(CC BY) license (http://creativecommons.org/licenses/by/4.0/).
... In addition to the Kalman filter, the unscented transform has been applied to the particle filter, which is referred to as the unscented particle filter (UPF) [36]. There exists also a square-root variant, which is the square-root UPF (SRUPF) [37]. ...
... The generation of sample points in (32), (37), and (47) is changed to ...
Article
Full-text available
Estimating the state of a system by fusing sensor data is a major prerequisite in many applications. When the state is time-variant, derivatives of the Kalman filter are a popular choice for solving that task. Two variants are the square-root unscented Kalman filter (SRUKF) and the square-root cubature Kalman filter (SCKF). In contrast to the unscented Kalman filter (UKF) and the cubature Kalman filter (CKF), they do not operate on the covariance matrix but on its square root. In this work, we modify the SRUKF and the SCKF for use on manifolds. This is particularly relevant for many state estimation problems when, for example, an orientation is part of a state or a measurement. In contrast to other approaches, our solution is both generic and mathematically coherent. It has the same theoretical complexity as the UKF and CKF on manifolds, but we show that the practical implementation can be faster. Furthermore, it gains the improved numerical properties of the classical SRUKF and SCKF. We compare the SRUKF and the SCKF on manifolds to the UKF and the CKF on manifolds, using the example of odometry estimation for an autonomous car. It is demonstrated that all algorithms have the same localization performance, but our SRUKF and SCKF have lower computational demands.
... However, the PF suffers from the widely-known problem of particle depletion, wherein a significant fraction of particles loses their weights during update. Although there are some methods to alleviate the particle depletion, such as selecting appropriate importance probability density [25][26][27][28][29] and resampling [30][31][32][33][34], the problem of particle depletion cannot be completely solved. ...
Preprint
Full-text available
This paper proposes a novel and efficient key conditional quotient filter (KCQF) for the estimation of state in the nonlinear system which can be either Gaussian or non-Gaussian, and either Markovian or non-Markovian. The core idea of the proposed KCQF is that only the key measurement conditions, rather than all measurement conditions, should be used to estimate the state. Based on key measurement conditions, the quotient-form analytical integral expressions for the conditional probability density function, mean, and variance of state are derived by using the principle of probability conservation, and are calculated by using the Monte Carlo method, which thereby constructs the KCQF. Two nonlinear numerical examples were given to demonstrate the superior estimation accuracy of KCQF, compared to seven existing filters.
... (18). The measurement innovation is defined as the error between the actual value of the measurement variable and its predicted value [37][38][39]: ...
Article
Full-text available
State estimation of a vehicle is an important direction under the research branch of automotive dynamics, with the aim of determining state variables that reflect vehicle handling stability and other characteristics. In order to solve the problem of poor estimation accuracy caused by heavy tailed non Gaussian noise in traditional state estimation methods, a new filtering algorithm based on the Maximum Correlation Entropy criterion (MCC) and the Square-root Cubature Kalman Filter (MCSCKF) is proposed. On the basis of establishing a nonlinear 3-DOF vehicle model, the yaw rate and the side slip angle as well as the longitudinal velocity of the vehicle were estimated. And the effectiveness of the algorithm was verified through joint simulation with Carsim and Matlab/Simulink. The results show that the MCSCKF algorithm can adapt to complex working conditions and has better accuracy in vehicle state estimation than traditional state estimation algorithms. Meanwhile, the MCSCKF algorithm can effectively reduce the impact of heavy tail non Gaussian noise and improve the accuracy of vehicle state estimation.
... The particle filter (PF) is a typical method used to handle nonlinear systems with non-Gaussian noises [15][16][17]. Instead of integral calculation, PF uses sample mean to implement the Bayesian estimation under nonlinear dynamics. When the particle number is sufficiently large, the posterior probability density function of particles will be sufficiently accurate to guarantee the accuracy of the state mean and variance. ...
Article
Full-text available
In vehicle navigation, it is quite common that the dynamic system is subject to various constraints, which increases the difficulty in nonlinear filtering. To address this issue, this paper presents a new constrained cubature particle filter (CCPF) for vehicle navigation. Firstly, state constraints are incorporated in the importance sampling process of the traditional cubature particle filter to enhance the accuracy of the importance density function. Subsequently, the Euclidean distance is employed to optimize the resampling process by adjusting particle weights to avoid particle degradation. Further, the convergence of the proposed CCPF is also rigorously proved, showing that the posterior probability function is converged when the particle number N → ∞. Our experimental results and the results of a comparative analysis regarding GNSS/DR (Global Navigation Satellite System/Dead Reckoning)-integrated vehicle navigation demonstrate that the proposed CCPF can effectively estimate system state under constrained conditions, leading to higher estimation accuracy than the traditional particle filter and cubature particle filter.
... SOC estimation using the UKF will be divergent because of its drawbacks, such as the pseudo-positive definiteness of the covariance matrix caused by the error accumulation in the UKF. To resolve these shortcomings, the square-root unscented Kalman filter (SRUKF) is presented to estimate the batteries' SOC [25][26][27][28][29]. Compared to the UKF, the SRUKF uses the state covariance square-root matrix instead of the state covariance matrix, which can overcome the problem of the pseudo-positive definiteness of the covariance matrix. ...
Article
Full-text available
Accurate state of charge (SOC) estimation is helpful for battery management systems to extend batteries’ lifespan and ensure the safety of batteries. However, due to the pseudo-positive definiteness of the covariance matrix and noise statistics error accumulation, the SOC estimation of lithium-ion batteries is usually inaccurate or even divergent using Kalman filters, such as the unscented Kalman filter (UKF) and the square-root unscented Kalman filter (SRUKF). To resolve this problem, an SOC estimation method based on the dual-coefficient tracking improved square-root unscented Kalman filter for lithium-ion batteries is developed. The method is composed of an improved square-root unscented Kalman filter (ISRUKF) and a dual-coefficient tracker. To avoid the divergence of SOC estimation due to the covariance matrix with pseudo-positive definiteness, an ISRUKF based on the QR decomposition covariance square-root matrix is presented. Moreover, the dual-coefficient tracker is designed to track and correct the state noise error of the battery, which can reduce the SOC estimation error caused by the accumulation of the battery model error using the ISRUKF. The accuracy and robustness of the SOC estimation method using the developed method are validated by the comparison with the UKF and SRUKF. The developed algorithm shows the highest SOC estimation accuracy with the SOC error within 1.5%.
... The filtering methods can be categorized as follows: the minimum mean-square error (MMSE) based methods, such as the extended Kalman filter (EKF) [17], the sampling-based methods, such as the unscented Kalman filter (UKF) [9]and particle filters (PF) [18], the artificial intelligence methods (AI) [19], such as the artificial neural networks or the adaptive neuro-fuzzy inference systems (ANFIS) [20], and the predictive filtering or model predictive filtering (MPF) [21] to track measurement output by using prediction output to estimate model error of the system. Also, there are some recent researches use the adaptive Kalman filter (AKF) for INS/GPS integration [22,23]. The AKF adjusts the value of the noise covariance matrices for the system (Q) and measurement (R) only, therefore, the AKF is not robust to model error. ...
Article
Full-text available
Resident Space Objects (RSO) are human-made objects in orbit around Earth and can remain there for an extended period. These objects can include active satellites, rockets, and space stations, as well as debris caused by previous space endeavours. Debris originated as a consequential outcome of activities such as space launches, orbital missions and collision events, pose a formidable threat to currently operational space assets. To reduce the risk of on-orbit collisions, it is imperative that spacecraft operators enhance their situational awareness concerning potential threats posed by RSO. This necessitates comprehensive tracking of the total number of objects in space and the continuous estimation of the probability of accidental collisions. Effective Collision Avoidance (CA) manoeuvres rely on accurate tracking and characterization of RSO. Currently, RSO are monitored and catalogued using ground-based observational systems. However, Space-Based Space Surveillance (SBSS) presents a viable solution for tracking the RSO, providing superior sensor resolution, tracking accuracy, and independence from weather conditions. Accurate and continuous orbit determination of RSO is critical for developing a robust framework that enables accurate prediction of RSO dynamics. This capability is essential for applications such as Interplanetary space exploration, space tourism and Point-To-Point Suborbital Transport (PPST), which are anticipated in the future. The current study proposes a multi-sensor data fusion strategy designed to integrate angular measurements extracted from image sequences obtained by multiple cost-effective Electro-Optical Sensors (EOS) sensors deployed in SBSS missions. The main contribution of this study lies in the development of data fusion frameworks tailored for constrained computational environments, ensuring seamless real-time implementation on intelligent Distributed Satellite Systems (iDSS). This study proposes and rigorously compares three distinct data fusion methodologies—Measurement Fusion-1 (MF-1), Measurement Fusion-2 (MF-2), and Track-to-Track (T2T) fusion—examining their impact on tracking accuracy across varying sensor-to-target geometries. Additionally, the data fusion framework is validated under diverse operational conditions, including Ground-Based Surveillance (GBS), SBSS, and the synergistic integration of GBS and SBSS. A validation case study is conducted on an iDSS constellation executing a SBSS mission. The results indicate that MF-1 outperforms other algorithms in the SBSS scenario in terms of tracking accuracy. In contrast, T2T fusion demonstrates superior performance in terms of computational time. Notably, the integration of SBSS and GBS data surpasses the performance of GBS across all evaluated data fusion methodologies.
Article
Visible Light Communication (VLC) is increasingly seen as a promising solution for indoor ranging systems. This study investigates user range estimation from a light source in a single-input single-output (SISO) system, considering input-signal-dependent shot noise (ISDSN). We apply extended Kalman filter (EKF), maximum likelihood (ML), and nonlinear least square (NLS) estimator techniques for range estimation and derive the Bayesian Cramér–Rao lower bound (BCRLB) to assess system performance. Monte Carlo simulations confirm that the proposed estimators align well with theoretical expectations. The results indicate that estimation error rises with higher ISDSN levels for all techniques and the BCRLB, with the EKF showing superior accuracy in range estimation compared to ML and NLS.
Article
Full-text available
We addressed the fusion estimation problem for nonlinear multisensory systems. Based on the Gauss–Hermite approximation and weighted least square criterion, an augmented high-dimension measurement from all sensors was compressed into a lower dimension. By combining the low-dimension measurement function with the particle filter (PF), a weighted measurement fusion PF (WMF-PF) is presented. The accuracy of WMF-PF appears good and has a lower computational cost when compared to centralized fusion PF (CF-PF). An example is given to show the effectiveness of the proposed algorithms.
Article
Full-text available
Efficiency and reliability are key issues when a star sensor operates in tracking mode. In the case of high attitude dynamics, the performance of existing attitude tracking algorithms degenerates rapidly. In this paper an extended Kalman filtering-based attitude tracking algorithm is presented. The star sensor is modeled as a nonlinear stochastic system with the state estimate providing the three degree-of-freedom attitude quaternion and angular velocity. The star positions in the star image are predicted and measured to estimate the optimal attitude. Furthermore, all the cataloged stars observed in the sensor field-of-view according the predicted image motion are accessed using a catalog partition table to speed up the tracking, called star mapping. Software simulation and night-sky experiment are performed to validate the efficiency and reliability of the proposed method.
Article
Full-text available
A new algorithm called maximum correntropy unscented Kalman filter (MCUKF) is proposed and applied to relative state estimation in space communication networks. As is well known, the unscented Kalman filter (UKF) provides an efficient tool to solve the non-linear state estimate problem. However, the UKF usually plays well in Gaussian noises. Its performance may deteriorate substantially in the presence of non-Gaussian noises, especially when the measurements are disturbed by some heavy-tailed impulsive noises. By making use of the maximum correntropy criterion (MCC), the proposed algorithm can enhance the robustness of UKF against impulsive noises. In the MCUKF, the unscented transformation (UT) is applied to obtain a predicted state estimation and covariance matrix, and a nonlinear regression method with the MCC cost is then used to reformulate the measurement information. Finally, the UT is adopted to the measurement equation to obtain the filter state and covariance matrix. Illustrative examples demonstrate the superior performance of the new algorithm.
Article
Full-text available
For loosely coupled INS/GPS integrated navigation systems with low-cost and low-accuracy microelectromechanical device inertial sensors, in order to obtain enough accuracy, a full-state nonlinear dynamic model rather than a linearized error model is much more preferable. Particle filters are particularly for nonlinear and non-Gaussian situations, but typical bootstrap particle filters (BPFs) and some improved particle filters (IPFs) such as auxiliary particle filters (APFs) and Gaussian particle filters (GPFs) cannot solve the mismatch between the importance function and the likelihood function very well. The predicted particles propagated through inertial navigation equations cannot be scattered with certainty within the effective range of current observation when there are large drift errors of the inertial sensors. Therefore, the current observation cannot play the correction role well and these particle filters are invalid to some extent. The proposed IPF firstly estimates the corresponding state bias errors according to the current observation and then corrects the bias errors of the predicted particles before determining the weights and resampling the particles. Simulations and practical experiments both show that the proposed IPF can effectively solve the mismatch between the importance function and the likelihood function of a BPF and compensate the accumulated errors of INSs very well. It has great robustness in a serious noisy scenario.
Article
Full-text available
The derivation of a conventional error model for the miniature gyroscope-based measurement while drilling (MGWD) system is based on the assumption that the errors of attitude are small enough so that the direction cosine matrix (DCM) can be approximated or simplified by the errors of small-angle attitude. However, the simplification of the DCM would introduce errors to the navigation solutions of the MGWD system if the initial alignment cannot provide precise attitude, especially for the low-cost microelectromechanical system (MEMS) sensors operated in harsh multilateral horizontal downhole drilling environments. This paper proposes a novel nonlinear error model (NNEM) by the introduction of the error of DCM, and the NNEM can reduce the propagated errors under large-angle attitude error conditions. The zero velocity and zero position are the reference points and the innovations in the states estimation of particle filter (PF) and Kalman filter (KF). The experimental results illustrate that the performance of PF is better than KF and the PF with NNEM can effectively restrain the errors of system states, especially for the azimuth, velocity, and height in the quasi-stationary condition.
Article
This paper is concerned with the distributed filtering problem for discrete-time nonlinear systems over a sensor network. In contrast with the distributed filters with linear consensus estimate, a distributed extended Kalman filter (EKF) is developed with nonlinear consensus estimate. Specifically, a new nonlinear consensus protocol with polynomial form is proposed to generate the consensus estimate. By using the variance-constrained approach, the Kalman gain matrix is determined for each node to guarantee an optimized upper bound on the state estimation error covariance despite consensus terms and linearization errors. It is shown that the Kalman gain matrix can be derived by solving two Riccati-like difference equations. The effectiveness of the proposed filter is evaluated on an indoor localization of a mobile robot with visual tracking systems.