# An Enhanced UWB-Based Range/GPS Cooperative Positioning Approach Using Adaptive Variational Bayesian Cubature Kalman Filtering

Authors: Feng Shen; Guanghui Xu
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

Precise position awareness is a fundamental requirement for advanced applications of emerging intelligent transportation systems, such as collision warning and speed advisory system. However, the achievable level of positioning accuracy using global navigation satellite systems does not meet the requirements of these applications. Fortunately, cooperative positioning (CP) techniques can improve the performance of positioning in a vehicular ad hoc network (VANET) through sharing the positions between vehicles. In this paper, a novel enhanced CP technique is presented by combining additional range-ultra-wide bandwidth- (UWB-) based measurements. Furthermore, an adaptive variational Bayesian cubature Kalman filtering (AVBCKF) algorithm is proposed and used in the enhanced CP method, which can add robustness to the time-variant measurement noise. Based on analytical and experimental results, the proposed AVBCKF-based CP method outperforms the cubature Kalman filtering- (CKF-) based CP method and extended Kalman filtering- (EKF-) based CP method.


---

## Body

## 1. Introduction

Maintaining the availability of absolute position is a significant challenge for many applications such as intelligent transportation systems (ITS) [1] and location based services (LBS) [2]. The Global Navigation Satellite Systems (GNSSs), such as the Global Positioning System (GPS), do not meet the requirements of safety-related applications, such as collision avoidance and lane-level guidance, due to limited accuracy and availability. Several techniques have been proposed to improve the GPS positioning performance, such as real-time kinematic (RTK) [3], differential GPS (DGPS) [4], and satellite/ground-based augmentation systems [5]. However, these techniques rely on the infrastructures and cannot perform well in urban environments due to limited view of the sky, multipath interference, and unavailability of line of sight. Fortunately, some innovative approaches called cooperative positioning (CP) have been proposed in recent years for positioning accuracy enhancement within vehicular networks, based on mutual communicating among the nodes of the network. The CP process typically consists of two phases [6]. The first phase is the measurement phase, during which the vehicles measure internal state information, such as vehicles’ positions by standalone GPS receivers, and range values by ranging instruments. The second phase is the position update phase, during which vehicles share the measurements with others and infer their positions based on all the measurements.In the first phase of CP, radio-based-ranging methods are the widely used ranging techniques which use signal propagation characteristics to derive an estimation of the distance between the transmitter and the receiver [7]. There are four common radio-ranging techniques used for CP in VANETs: time of arrival (TOA), time difference of arrival (TDOA), angle of arrival (AOA), and received signal strength (RSS). Christie et al. [8] combined the RSS based intervehicle distance measurements with the road maps and vehicle kinematics for vehicle positioning. However, the RSS-based range does not always provide accurate distance measurement due to large variations in signal attenuation under different environments, particularly when multipath and shadowing effects are present [9]. Benslimane [10] assumed that the ranges among vehicles can be estimated using RSS, TOA, or TDOA and proposed a CP method that uses the ranges and GPS-based positions to estimate other GPS-denied positions. In [11, 12], two range-based CP methods are proposed to mitigate GPS multipath error and improve accuracy in a VANET, respectively. Apart from the range-based CP techniques, there is another class of CP techniques, based on radio-ranging rate, which was presented in [13–15]. In [13], a location estimation algorithm based on the Doppler effect is presented to estimate the position of a moving target in a wireless sensor network. However, the geometry of the target over a large distance is not consistent with the geometry of the vehicular environment in urban streets, which is not suitable for vehicle positioning. A Doppler-shift-based CP method for VANETs is proposed in [14], which mainly relies on the infrastructure.In the second phase, measurements are aggregated and passed to the fusing algorithm. For most of the CP data fusion, the system and measurement models are nonlinear; therefore, the nonlinear filtering techniques are used to achieve better positioning results. As a widely used nonlinear filtering technique, extended Kalman filter (EKF) is usually used as the core of the CP algorithm [16, 17]. However, the EKF can result in particularly poor performance if the dynamic systems are highly nonlinear. As better alternatives to the EKF, some other filters based on Bayesian sampling, such as unscented Kalman filter (UKF) and particle filter [18, 19], can also be used for nonlinear system. However, a heavy computational load and the curse of the dimensionality in practical application are major constraints in these filtering algorithms. The cubature Kalman filter (CKF) [20] has recently received increasing attention, which employs deterministic sampling to evaluate the intractable integrals encountered in filtering problem. However, the performance of these filters mentioned above may degrade due to the fact that in practical situations the statistics of measurement noise might change.In this paper, an enhanced CP method by utilizing the ultra-wideband- (UWB-) based-range measurements is proposed. It focuses on the absolute positioning in emerging intelligent transportation systems. In this method, the GPS-based positions are shared among the participating vehicles. Then each vehicle fuses the GPS measurements and UWB-based range to obtain the position. For data fusion, an adaptive variational Bayesian cubature Kalman filtering (AVBCKF) is proposed as the core of the CP algorithm. In each update step of the AVBCKF, both the system state and time-variant measurement noise are considered as random variables to be estimated. Measurement noise variance is approximated by variational Bayesian (VB) approach; thereafter, system states are updated by cubature Kalman filtering. The analytical and experimental results show that the AVBCKF can dynamically estimate the measurement noise, and the AVBCKF-based CP method outperforms CKF-based CP method and EKF-based CP method.The rest of the paper is organized as follows. In Section2, the novel AVBCKF is described. Section 3 explains the UWB-based-range technique and the estimator of the proposed CP method. In Section 4, the experimental results are discussed, and the performance of the proposed system is evaluated. Section 5 summarizes the contributions of this work and future work.


## 2. Adaptive Variational Bayesian Cubature Kalman Filtering

### 2.1. Variational Bayesian Approach

The VB approach, also known as ensemble learning, takes its name from Feynman’s variational free energy method developed in statistical physics [21]. VB is developed by the machine learning community and has been applied in a variety of statistical and signal processing domains. In this paper, VB is used to estimate the noise variance of measurements in the proposed CP algorithm. Therefore, the following will explain the VB approach.In the parameter estimation domain, after getting the observed data setZ, the posterior probability density function (PDF) of parameter set θ  (
p
θ
∣
Z
) can be calculated by Bayes’ rule as
(1)
p
θ
∣
Z
=
p
Z
∣
θ
p
θ
p
Z
,
where p
Z is the marginal likelihood function and is the key parameter to obtain the value of p
θ
∣
Z. As a result of the intractability of the integral in computing p
Z, it is difficult to calculate p
Z to obtain p
θ
∣
Z. The key idea of VB is to use a new distribution q
θ to approximate the true distribution p
θ
∣
Z. The form of the new distribution q
θ is selected freely owing to the conjugacy property. Furthermore, if the set θ is partitioned into N parts as θ
=
{
θ
1
,
θ
2
,
…
,
θ
i
,
…
,
θ
N
}, then each θ
i is assumed to be independent of each other. Naturally q
θ factorizes into N independent q
θ
i, and the joint distribution as
(2)
q
θ
=
∏
i
=
1
N
q
θ
i
.
The technique used in VB approach is also called mean field variational Bayes.In VB framework, the goal is to find aq
θ which is as close as to p
θ
∣
Z. Fortunately, Kullback-Leibler (KL) divergence is a nonnegative dissimilarity function measuring the discrepancy between two distributions q
θ and p
θ
∣
Z. Hence q
θ can be obtained by minimizing the KL divergence between q
θ and p
θ
∣
Z. It has also been shown in [22] that minimizing KL
q
θ
∥
p
θ
∣
Z is equivalent to maximizing the lower bound of log
⁡
⁡
p
Z. The lower bound function can be written as
(3)
F
q
θ
=
∫
q
θ
log
⁡
⁡
p
Z
,
θ
q
θ
d
θ
.
Maximizing F
q
θ tightens the lower bound and gives rise to the optimal distribution. The approximate distribution is optimal in the sense of KL divergence because KL
q
θ
∥
p
θ
∣
Z equals zero if and only if q
θ
=
p
θ
∣
Z. It is worth noting that this optimal sense differs from the counterpart in optimal estimation theory, where the optimization is measured by minimum-variance rules. Therefore, q
θ
i can be computed by differentiating F
q
θ with respect to q
θ
j
≠
i.General solution ofq
θ
is is given as
(4)
q
θ
i
=
exp
⁡
⁡
E
q
θ
j
≠
i
log
⁡
⁡
p
Z
,
θ
∫
exp
⁡
⁡
E
q
θ
j
≠
i
log
⁡
⁡
p
Z
,
θ
d
θ
k
,
where E
q
θ
j
≠
i
log
⁡
⁡
p
Z
,
θ is the expectation of log joint distribution over all the parameters not in the partition.Note that (4) is an implicit solution because of the circular dependencies. This forms an iterative estimation scheme, where the distribution of each parameter is estimated with the expectation over other distributions, given the appropriate initialization of the hyperparameters. Afterwards, the distribution of each parameter is updated according to (4) during the next iterations, until the algorithm converges. The convergence of VB has been analytically proved in [23]. Hence, in VB framework coupled equations can be solved through form separable approximate distributions.


### 2.2. Cubature Kalman Filtering

Consider the discrete-time nonlinear filtering problems with additive process and measurement noise, whose state space model can be expressed by the pair difference equations, given as(5)
x
k
=
f
x
k
-
1
+
w
k
,
(6)
z
k
=
h
x
k
+
v
k
.
Equations (5) and (6) are the process equation and the measurement equation, respectively, where x
k and z
k are the state and measurement values at time k, respectively; f
x
k
-
1 and h
x
k are some nonlinear functions; w
k and v
k are white noise with zero mean and covariance q
k and r
k, respectively. From the perspective of probability, the state space model can be reformulated according to [24] as
(7)
x
k
~
p
x
k
∣
x
k
-
1
=
N
f
x
k
-
1
,
q
k
,
z
k
~
p
z
k
∣
x
k
=
N
h
x
k
,
r
k
.
Assume that x
k is Gaussian distribution with mean m
k and covariance P
k. Then, the prior density of the state at k
-
1 and k obeys the Gaussian distribution as p
x
k
∣
z
1
:
k
-
1
=
N
m
k
-
,
P
k
- and p
x
k
∣
z
1
:
k
=
N
m
k
,
P
k, respectively. Therefore, the prediction and update equations of the CKF can be expressed as follows.The prediction equation is as follows:(8)
χ
k
-
1
(
i
)
=
m
k
-
1
+
n
P
k
-
1
,
X
k
(
i
)
=
f
χ
k
-
1
(
i
)
,
i
=
1
,
…
,
2
n
,
m
k
-
=
1
2
n
∑
i
=
1
2
n
X
k
(
i
)
,
P
k
-
=
1
2
n
∑
i
=
1
2
n
X
k
i
-
m
k
-
X
k
i
-
m
k
-
T
+
q
k
.
The update equation is as follows:
(9)
χ
k
-
(
i
)
=
m
k
-
+
n
P
k
-
,
z
k
(
i
)
=
h
χ
k
-
(
i
)
,
i
=
1
,
…
,
2
n
,
K
k
=
1
2
n
∑
i
=
1
2
n
χ
k
-
i
-
m
k
-
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
·
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
∑
i
=
1
2
n
z
k
i
+
r
k
-
1
,
m
k
=
m
k
-
+
K
k
z
k
-
1
2
n
∑
i
=
1
2
n
z
k
(
i
)
,
P
k
=
P
k
-
-
K
k
·
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
∑
i
=
1
2
n
z
k
i
+
r
k
K
k
T
.


### 2.3. The Proposed Adaptive Variational Bayesian Cubature Kalman Filtering

In the traditional CKF algorithm, the statistical moment of measurement noise is invariant. However, the performance of the traditional CKF may be degraded due to the fact that in practical situations the statistics of measurement noise might change with time. In order to introduce robustness against the measurement noise in the CP, an improved cubature Kalman filtering approach based on variational Bayesian is proposed as the core component of the CP algorithm. In each update step of the AVBCKF, both system state and time-variant measurement noise are considered as random variables to be estimated. Measurements noise variances are approximated by variational Bayesian approach; thereafter, the system states are updated by cubature Kalman filtering.In the proposed AVBCKF algorithm, the system statex
k and the covariance of time-variant measurement noise r
k are considered as random variables to be estimated. Consequently, the prior distribution of the joint probability density function of x
k and r
k at time k
-
1 can be expressed as
(10)
p
x
k
,
r
k
∣
z
1
:
k
-
1
=
∫
p
x
k
∣
x
k
-
1
p
r
k
∣
r
k
-
1
·
p
x
k
-
1
,
r
k
-
1
∣
z
1
:
k
-
1
d
x
k
-
1
d
r
k
-
1
.
At time k, the prior distribution of the joint probability density function of x
k and r
k is
(11)
p
x
k
,
r
k
∣
z
1
:
k
=
p
x
k
,
r
k
,
z
1
:
k
p
z
1
:
k
=
p
z
k
∣
x
k
,
r
k
p
x
k
,
r
k
∣
z
1
:
k
-
1
p
z
k
∣
z
1
:
k
-
1
.
From the perspective of generalized Bayes’ rule, (10) and (11) can be seen as the prediction equation and the update equation of Bayes filtering theory. However, the computation of the integral in (10) is intractability, and the variational Bayesian approach is used to obtain the approximate optimal value. Assume that the variables x
k and r
k are independent of each other, and, according to the prior knowledge [25], these can obey the Gaussian distribution and inverse-gamma distribution, respectively. Then, at time k
-
1,
(12)
p
x
k
,
r
k
∣
z
1
:
k
-
1
=
p
x
k
∣
z
1
:
k
-
1
p
r
k
∣
z
1
:
k
-
1
=
N
x
k
∣
m
k
-
,
P
k
-
∏
i
=
1
d
IG
σ
k
,
i
2
∣
α
k
,
i
-
,
β
k
,
i
-
.After getting the measurement at timek, a new distribution q
x
k
,
r
k
∣
z
1
:
k to replace the real posterior distribution p
x
k
,
r
k
∣
z
1
:
k will be introduced to use the variational Bayesian approach. For simplicity, the dependence of distribution on z is omitted in the following formulations. For example, the q
x
k
,
r
k
∣
z
1
:
k will be expressed as q
x
k
,
r
k. Under the assumption that two variables x
k and r
k are independent, the following relationship can be obtained q
x
k
,
r
k
=
q
x
k
q
r
k. Equation (4) gives the general approximate solution of variational Bayesian approach. Meanwhile, referring to the CKF algorithm and using the nonlinear function h
· in (6), for the state variable x
k and the covariance of measurement noise r
k, the following relationship can be obtained:
(13)
q
x
k
=
C
exp
⁡
-
1
2
x
k
T
P
k
-
-
1
+
H
k
T
r
^
k
-
1
H
k
x
k
1
2
+
x
k
T
P
k
-
-
1
m
k
-
+
H
k
T
r
^
k
-
1
z
k
,
(14)
q
r
k
=
C
exp
⁡
∑
i
=
1
d
-
α
k
,
i
-
-
1
2
-
1
ln
⁡
σ
k
,
i
2
-
∑
i
=
1
d
1
σ
k
,
i
2
β
k
,
i
-
+
1
2
E
x
k
z
k
-
h
k
x
k
i
2

,
where C is the constant and is unrelated to the form of distribution. As it can be seen from (13) and (14), the posterior distributions, q
x
k and q
r
k, have the same form and different parameters with the prior distributions, p
x
k
∣
z
1
:
k
-
1 and p
r
k
∣
z
1
:
k
-
1, which obey the Gaussian distribution and inverse-gamma distribution, respectively. The reason is that the Gaussian distribution and inverse-gamma distribution belong to the conjugate exponential distribution [26].The parametersm
k, P
k in the Gaussian distribution of state variable can be derived as
(15)
m
k
=
m
k
-
+
K
^
k
z
k
-
1
2
n
∑
i
=
1
2
n
z
k
(
i
)
,
P
k
=
P
k
-
-
K
^
k
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
+
r
^
k
K
k
T
,
K
^
k
=
1
2
n
∑
i
=
1
2
n
χ
k
-
i
-
m
k
-
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
·
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
+
r
^
k
-
1
,
where
(16)
r
^
k
=
E
r
k
=
diag
⁡
E
σ
k
,
1
2
,
E
σ
k
,
2
2
,
…
,
E
σ
k
,
d
2
and the parameters in approximate inverse-gamma distribution q
r
k
=
∏
i
=
1
d
IG
σ
k
,
i
2
∣
α
k
,
i
,
β
k
,
i can also be derived as
(17)
α
k
,
i
=
α
k
,
i
-
+
1
2
,
(18)
β
k
,
i
=
β
k
,
i
-
+
1
2
E
x
k
z
k
-
h
k
x
k
i
2
.
Using the characteristic of inverse-gamma distribution [27],
(19)
E
σ
k
,
i
2
=
β
k
,
i
α
k
,
i
,
i
=
1
~
d
.
According to the sample strategy of CKF, the expectation in (18) can be derived as
(20)
E
x
k
z
k
-
h
k
x
k
i
2
=
z
k
-
1
2
n
∑
i
=
1
2
n
h
m
k
-
+
n
P
k
-
i
2
.The joint density ofx
k and r
k can be estimated using (13) to (20). The steps of variational Bayesian algorithm can be summarized as follows. Firstly, the parameters of the prior distribution in (15), (17), (18), and (19) are defined to calculate the approximate distribution q
r
k and the parameters m
k, P
k. Then the distribution is updated as shown in (13) and (14), and new values are calculated using (19) and (20). Furthermore, the distribution parameters are updated using (15) to (18). The estimation process is iterative, and the iteration will stop when the convergence is reached. It is reported in [27] that in a similar case VB will converge very soon with only a few iterations.After using the VB to estimate the covariance of measurement noise and combining the VB into CKF, the AVBCKF proposed in this paper can be explained as follows.(i)
Predict:
Initialize theα
0, β
0, m
0, P
0.
Predict the noise parameters using (17), (18), and (8).
(ii)
Update:
Update the measurement noise using VB equations (14) and (17)-(18), and then update the state variable using (15).


## 2.1. Variational Bayesian Approach

The VB approach, also known as ensemble learning, takes its name from Feynman’s variational free energy method developed in statistical physics [21]. VB is developed by the machine learning community and has been applied in a variety of statistical and signal processing domains. In this paper, VB is used to estimate the noise variance of measurements in the proposed CP algorithm. Therefore, the following will explain the VB approach.In the parameter estimation domain, after getting the observed data setZ, the posterior probability density function (PDF) of parameter set θ  (
p
θ
∣
Z
) can be calculated by Bayes’ rule as
(1)
p
θ
∣
Z
=
p
Z
∣
θ
p
θ
p
Z
,
where p
Z is the marginal likelihood function and is the key parameter to obtain the value of p
θ
∣
Z. As a result of the intractability of the integral in computing p
Z, it is difficult to calculate p
Z to obtain p
θ
∣
Z. The key idea of VB is to use a new distribution q
θ to approximate the true distribution p
θ
∣
Z. The form of the new distribution q
θ is selected freely owing to the conjugacy property. Furthermore, if the set θ is partitioned into N parts as θ
=
{
θ
1
,
θ
2
,
…
,
θ
i
,
…
,
θ
N
}, then each θ
i is assumed to be independent of each other. Naturally q
θ factorizes into N independent q
θ
i, and the joint distribution as
(2)
q
θ
=
∏
i
=
1
N
q
θ
i
.
The technique used in VB approach is also called mean field variational Bayes.In VB framework, the goal is to find aq
θ which is as close as to p
θ
∣
Z. Fortunately, Kullback-Leibler (KL) divergence is a nonnegative dissimilarity function measuring the discrepancy between two distributions q
θ and p
θ
∣
Z. Hence q
θ can be obtained by minimizing the KL divergence between q
θ and p
θ
∣
Z. It has also been shown in [22] that minimizing KL
q
θ
∥
p
θ
∣
Z is equivalent to maximizing the lower bound of log
⁡
⁡
p
Z. The lower bound function can be written as
(3)
F
q
θ
=
∫
q
θ
log
⁡
⁡
p
Z
,
θ
q
θ
d
θ
.
Maximizing F
q
θ tightens the lower bound and gives rise to the optimal distribution. The approximate distribution is optimal in the sense of KL divergence because KL
q
θ
∥
p
θ
∣
Z equals zero if and only if q
θ
=
p
θ
∣
Z. It is worth noting that this optimal sense differs from the counterpart in optimal estimation theory, where the optimization is measured by minimum-variance rules. Therefore, q
θ
i can be computed by differentiating F
q
θ with respect to q
θ
j
≠
i.General solution ofq
θ
is is given as
(4)
q
θ
i
=
exp
⁡
⁡
E
q
θ
j
≠
i
log
⁡
⁡
p
Z
,
θ
∫
exp
⁡
⁡
E
q
θ
j
≠
i
log
⁡
⁡
p
Z
,
θ
d
θ
k
,
where E
q
θ
j
≠
i
log
⁡
⁡
p
Z
,
θ is the expectation of log joint distribution over all the parameters not in the partition.Note that (4) is an implicit solution because of the circular dependencies. This forms an iterative estimation scheme, where the distribution of each parameter is estimated with the expectation over other distributions, given the appropriate initialization of the hyperparameters. Afterwards, the distribution of each parameter is updated according to (4) during the next iterations, until the algorithm converges. The convergence of VB has been analytically proved in [23]. Hence, in VB framework coupled equations can be solved through form separable approximate distributions.


## 2.2. Cubature Kalman Filtering

Consider the discrete-time nonlinear filtering problems with additive process and measurement noise, whose state space model can be expressed by the pair difference equations, given as(5)
x
k
=
f
x
k
-
1
+
w
k
,
(6)
z
k
=
h
x
k
+
v
k
.
Equations (5) and (6) are the process equation and the measurement equation, respectively, where x
k and z
k are the state and measurement values at time k, respectively; f
x
k
-
1 and h
x
k are some nonlinear functions; w
k and v
k are white noise with zero mean and covariance q
k and r
k, respectively. From the perspective of probability, the state space model can be reformulated according to [24] as
(7)
x
k
~
p
x
k
∣
x
k
-
1
=
N
f
x
k
-
1
,
q
k
,
z
k
~
p
z
k
∣
x
k
=
N
h
x
k
,
r
k
.
Assume that x
k is Gaussian distribution with mean m
k and covariance P
k. Then, the prior density of the state at k
-
1 and k obeys the Gaussian distribution as p
x
k
∣
z
1
:
k
-
1
=
N
m
k
-
,
P
k
- and p
x
k
∣
z
1
:
k
=
N
m
k
,
P
k, respectively. Therefore, the prediction and update equations of the CKF can be expressed as follows.The prediction equation is as follows:(8)
χ
k
-
1
(
i
)
=
m
k
-
1
+
n
P
k
-
1
,
X
k
(
i
)
=
f
χ
k
-
1
(
i
)
,
i
=
1
,
…
,
2
n
,
m
k
-
=
1
2
n
∑
i
=
1
2
n
X
k
(
i
)
,
P
k
-
=
1
2
n
∑
i
=
1
2
n
X
k
i
-
m
k
-
X
k
i
-
m
k
-
T
+
q
k
.
The update equation is as follows:
(9)
χ
k
-
(
i
)
=
m
k
-
+
n
P
k
-
,
z
k
(
i
)
=
h
χ
k
-
(
i
)
,
i
=
1
,
…
,
2
n
,
K
k
=
1
2
n
∑
i
=
1
2
n
χ
k
-
i
-
m
k
-
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
·
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
∑
i
=
1
2
n
z
k
i
+
r
k
-
1
,
m
k
=
m
k
-
+
K
k
z
k
-
1
2
n
∑
i
=
1
2
n
z
k
(
i
)
,
P
k
=
P
k
-
-
K
k
·
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
∑
i
=
1
2
n
z
k
i
+
r
k
K
k
T
.


## 2.3. The Proposed Adaptive Variational Bayesian Cubature Kalman Filtering

In the traditional CKF algorithm, the statistical moment of measurement noise is invariant. However, the performance of the traditional CKF may be degraded due to the fact that in practical situations the statistics of measurement noise might change with time. In order to introduce robustness against the measurement noise in the CP, an improved cubature Kalman filtering approach based on variational Bayesian is proposed as the core component of the CP algorithm. In each update step of the AVBCKF, both system state and time-variant measurement noise are considered as random variables to be estimated. Measurements noise variances are approximated by variational Bayesian approach; thereafter, the system states are updated by cubature Kalman filtering.In the proposed AVBCKF algorithm, the system statex
k and the covariance of time-variant measurement noise r
k are considered as random variables to be estimated. Consequently, the prior distribution of the joint probability density function of x
k and r
k at time k
-
1 can be expressed as
(10)
p
x
k
,
r
k
∣
z
1
:
k
-
1
=
∫
p
x
k
∣
x
k
-
1
p
r
k
∣
r
k
-
1
·
p
x
k
-
1
,
r
k
-
1
∣
z
1
:
k
-
1
d
x
k
-
1
d
r
k
-
1
.
At time k, the prior distribution of the joint probability density function of x
k and r
k is
(11)
p
x
k
,
r
k
∣
z
1
:
k
=
p
x
k
,
r
k
,
z
1
:
k
p
z
1
:
k
=
p
z
k
∣
x
k
,
r
k
p
x
k
,
r
k
∣
z
1
:
k
-
1
p
z
k
∣
z
1
:
k
-
1
.
From the perspective of generalized Bayes’ rule, (10) and (11) can be seen as the prediction equation and the update equation of Bayes filtering theory. However, the computation of the integral in (10) is intractability, and the variational Bayesian approach is used to obtain the approximate optimal value. Assume that the variables x
k and r
k are independent of each other, and, according to the prior knowledge [25], these can obey the Gaussian distribution and inverse-gamma distribution, respectively. Then, at time k
-
1,
(12)
p
x
k
,
r
k
∣
z
1
:
k
-
1
=
p
x
k
∣
z
1
:
k
-
1
p
r
k
∣
z
1
:
k
-
1
=
N
x
k
∣
m
k
-
,
P
k
-
∏
i
=
1
d
IG
σ
k
,
i
2
∣
α
k
,
i
-
,
β
k
,
i
-
.After getting the measurement at timek, a new distribution q
x
k
,
r
k
∣
z
1
:
k to replace the real posterior distribution p
x
k
,
r
k
∣
z
1
:
k will be introduced to use the variational Bayesian approach. For simplicity, the dependence of distribution on z is omitted in the following formulations. For example, the q
x
k
,
r
k
∣
z
1
:
k will be expressed as q
x
k
,
r
k. Under the assumption that two variables x
k and r
k are independent, the following relationship can be obtained q
x
k
,
r
k
=
q
x
k
q
r
k. Equation (4) gives the general approximate solution of variational Bayesian approach. Meanwhile, referring to the CKF algorithm and using the nonlinear function h
· in (6), for the state variable x
k and the covariance of measurement noise r
k, the following relationship can be obtained:
(13)
q
x
k
=
C
exp
⁡
-
1
2
x
k
T
P
k
-
-
1
+
H
k
T
r
^
k
-
1
H
k
x
k
1
2
+
x
k
T
P
k
-
-
1
m
k
-
+
H
k
T
r
^
k
-
1
z
k
,
(14)
q
r
k
=
C
exp
⁡
∑
i
=
1
d
-
α
k
,
i
-
-
1
2
-
1
ln
⁡
σ
k
,
i
2
-
∑
i
=
1
d
1
σ
k
,
i
2
β
k
,
i
-
+
1
2
E
x
k
z
k
-
h
k
x
k
i
2

,
where C is the constant and is unrelated to the form of distribution. As it can be seen from (13) and (14), the posterior distributions, q
x
k and q
r
k, have the same form and different parameters with the prior distributions, p
x
k
∣
z
1
:
k
-
1 and p
r
k
∣
z
1
:
k
-
1, which obey the Gaussian distribution and inverse-gamma distribution, respectively. The reason is that the Gaussian distribution and inverse-gamma distribution belong to the conjugate exponential distribution [26].The parametersm
k, P
k in the Gaussian distribution of state variable can be derived as
(15)
m
k
=
m
k
-
+
K
^
k
z
k
-
1
2
n
∑
i
=
1
2
n
z
k
(
i
)
,
P
k
=
P
k
-
-
K
^
k
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
+
r
^
k
K
k
T
,
K
^
k
=
1
2
n
∑
i
=
1
2
n
χ
k
-
i
-
m
k
-
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
·
1
2
n
∑
i
=
1
2
n
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
z
k
i
-
1
2
n
∑
i
=
1
2
n
z
k
i
T
+
r
^
k
-
1
,
where
(16)
r
^
k
=
E
r
k
=
diag
⁡
E
σ
k
,
1
2
,
E
σ
k
,
2
2
,
…
,
E
σ
k
,
d
2
and the parameters in approximate inverse-gamma distribution q
r
k
=
∏
i
=
1
d
IG
σ
k
,
i
2
∣
α
k
,
i
,
β
k
,
i can also be derived as
(17)
α
k
,
i
=
α
k
,
i
-
+
1
2
,
(18)
β
k
,
i
=
β
k
,
i
-
+
1
2
E
x
k
z
k
-
h
k
x
k
i
2
.
Using the characteristic of inverse-gamma distribution [27],
(19)
E
σ
k
,
i
2
=
β
k
,
i
α
k
,
i
,
i
=
1
~
d
.
According to the sample strategy of CKF, the expectation in (18) can be derived as
(20)
E
x
k
z
k
-
h
k
x
k
i
2
=
z
k
-
1
2
n
∑
i
=
1
2
n
h
m
k
-
+
n
P
k
-
i
2
.The joint density ofx
k and r
k can be estimated using (13) to (20). The steps of variational Bayesian algorithm can be summarized as follows. Firstly, the parameters of the prior distribution in (15), (17), (18), and (19) are defined to calculate the approximate distribution q
r
k and the parameters m
k, P
k. Then the distribution is updated as shown in (13) and (14), and new values are calculated using (19) and (20). Furthermore, the distribution parameters are updated using (15) to (18). The estimation process is iterative, and the iteration will stop when the convergence is reached. It is reported in [27] that in a similar case VB will converge very soon with only a few iterations.After using the VB to estimate the covariance of measurement noise and combining the VB into CKF, the AVBCKF proposed in this paper can be explained as follows.(i)
Predict:
Initialize theα
0, β
0, m
0, P
0.
Predict the noise parameters using (17), (18), and (8).
(ii)
Update:
Update the measurement noise using VB equations (14) and (17)-(18), and then update the state variable using (15).


## 3. The Estimator of the Proposed CP

Assume that a VANET consists of a number of vehicles and all the vehicles are equipped with a GPS receiver and an UWB transceiver that can communicate the measurement data among the vehicles. The ultimate goal is that each vehicle can improve the accuracy of its position using a data fusion algorithm which is fed by local measurements and neighbors’ GPS-based positions and the UWB-based ranges. Therefore, the measurements used in the proposed CP include the positions and ranges of the vehicles. In the method, GPS-based positions are shared between vehicles through the UWB communication. The positions are combined with the range measurements from UWB to achieve enhanced positioning. The AVBCKF algorithm proposed above is designed as the core of the CP algorithm for data fusion. For simplicity and convenience, only two vehiclesk and l are considered, which can be easily extended to the case of more than two vehicles. In this paper, the orthogonal axes east-north-up (ENU) are used as the coordinate frame to express the location of the vehicle. For simplicity, 2D positioning is considered, but the method can easily be expanded to 3D positioning. Figure 1 shows the typical architecture of the proposed CP with two vehicles. As shown in Figure 1, assume that vehicle k is the target vehicle, and it broadcasts its GPS-based position x
,
y to vehicle l and receives the position x
l
,
y
l of vehicle l through UWB. Meanwhile, the UWB device in vehicle k measures the range R
l between vehicles k and l. In vehicle k, the positions x
,
y and x
l
,
y
l and the range R
l are used as inputs to the fusing algorithm to get the enhanced position of vehicle k.Figure 1
The architecture of CP in VANET.The state space model of the movement of target vehiclek is defined as
(21)
X
t
+
τ
=
F
X
t
+
G
W
t
,
where X
=
x
y
v
x
v
y
a
x
a
y
T is the state vector that contains the position x
,
y, velocity v
x
,
v
y, and acceleration a
x
,
a
y of the target vehicle; x and y are along the east and north axes, respectively. F is the state transition model, G is the process noise model, W is the Gaussian system noise with the STD σ and zero mean along each axis, the covariance of process noise is Q
=
σ
2
G
G
T, τ is the observation period, and t is the time. F and G are given as
(22)
F
=
I
τ
I
0.5
τ
2
I
O
I
τ
I
O
O
O
,
G
=
0.5
τ
2
I
τ
I
I
,
where I is a 2
×
2 identity matrix and O is a 2
×
2 zero matrix.Corresponding to the system state model, the observation model of the CP algorithm can be defined as(23)
Z
t
=
h
X
t
+
ζ
t
,
where h is a nonlinear observation vector in terms of X and ζ is the observation noise. The observation vector Z consists of GPS-based positions of two vehicles and the range between two vehicles measured by UWB. The distance R
l between vehicles k and l can be expressed as
(24)
R
l
=
x
-
x
l
2
+
y
-
y
l
2
.
EKF can be used to implement the CP algorithm due to nonlinearity of h in (23). However, using EKF introduces the prerequisites of definite models and known invariant noise parameters. In this paper, the AVBCKF is designed as the data fusion algorithm in CP which is robust to the variations in the measurement noise.


## 4. Experimental Results

In order to evaluate the performance of CP method based on the proposed AVBCKF, the same experimental data as found in [28] is used. The experimental setup includes two vehicles equipped with GPS receivers, INSs, and UWB transceivers. A set of relatively expensive reference equipment and a set of low cost sensors are fitted into each test vehicle. The reference equipment is the Leica GS10 receiver for vehicle 1 and a Novatel INS-LCI (integrated GNSS-INS) for vehicle 2. The carrier-phase-based differential position estimates (RTK) of these receivers were used as ground truth positioning data. The noisy GPS-based positions and the UWB- (MSSI-) based range were used as measurements to the AVBCKF-based data fusing technique in CP algorithm. Figure 2 shows the photograph of two experimental vehicles.Figure 2
Two experimental vehicles.The duration of the entire experiment was much larger than the duration of the data selected (14 minutes); however, this was the largest continuous block of data with corresponding RTK GPS availability. Meanwhile, two vehicles can communicate through UWB continuously during the experimental data selected. The GPS observations logged at the Nottingham Geospatial Institute reference station were used for the calculation of the DGPS corrections. The test area was the Clifton Boulevard, between Derby Road and Loughborough Road in Nottingham, UK, which had good open sky to maximize the satellite visibility. Figure3 shows the number of the common visible satellites to the vehicles for the selected experimental data. It can be found that the number of common visible satellites is almost always above the required minimum, that is, four. However, when it falls below the minimum, the Kalman filter can compensate using the dynamic model of the system, setting the innovation of missing observation to zero and assigning a large number, as infinity, to the corresponding element in observation covariance.Figure 3
Common satellite visibility of the experimental data selected.In addition to the AVBCKF, the EKF and CKF are also used as the data fusion algorithms in the CP to evaluate a relative efficiency of the proposed AVBCKF. For convenience, “EKF-CP,” “CKF-CP,” and “AVBCKF-CP” are used in the following figures to denote the three different data fusion algorithms mentioned above. The positioning error of vehicle was calculated ase
t
=
p
′
t
-
p
t, where p
′
t and p
t are the estimated and the true positions using RTK GPS data, respectively.Figures4 and 5 provide a comparison of positioning error along the east and north directions and positioning results against different data fusing algorithms.Figure 4
Comparison of positioning error in east direction with different data fusing algorithms.Figure 5
Comparison of positioning error in north direction with different data fusing algorithms.The root mean square (RMS) ofe
d (rmse) is evaluated according to the definition given in [28]. Figure 6 shows thermse of positioning results using different data fusing algorithms.Figure 6
Comparison ofrmse in positioning result with different data fusing algorithms.It can be noticed from the figures that, compared to the EKF-based data fusion and CKF-based data fusion, the proposed AVBCKF solution has improved precision of the position. The obvious reason is the consideration of both system state and time-variant measurement noise as random variables that are estimated in the proposed AVBCKF. Therefore, the AVBCKF-based data fusion algorithm is more robust to the variations in the measurement noise and outperforms the other two data fusing algorithms.


## 5. Conclusion

In this paper, an adaptive variational Bayesian cubature Kalman filtering is proposed as the CP algorithm. The major focus of the algorithm is to improve the performance of absolute position estimation in emerging intelligent transportation systems. In each update step of the proposed AVBCKF, both the system state and time-variant measurement noise are considered as random variables to be estimated, and the variances of measurement noise are approximated using variational Bayesian (VB) approach. Thereafter, the system states are updated by cubature Kalman filtering. The AVBCKF-based data fusion algorithm is shown to be robust to the variations in the measurement noise. The experimental results show that the AVBCKF-based CP method outperforms the CKF-based CP method and EKF-based CP method.



---

*Source: 843719-2015-01-18.xml*