Research Article Fusion Estimation Algorithm with Uncertain Noises and Its Application ... · 2019. 5. 2. · Research Article Fusion Estimation Algorithm with Uncertain Noises and

Research ArticleFusion Estimation Algorithm with Uncertain Noises andIts Application in Navigation System

Zhiping Ren1 and Huili Wang2

1School of Electronic Engineering, Xi’an Shiyou University, Xi’an, Shaanxi 710065, China2School of Automation, Northwestern Polytechnical University, Xi’an, Shaanxi 710072, China

Correspondence should be addressed to Zhiping Ren; [email protected]

Received 30 April 2014; Revised 9 September 2014; Accepted 9 September 2014

Academic Editor: Moran Wang

Copyright © 2015 Z. Ren and H. Wang.This is an open access article distributed under the Creative Commons Attribution License,which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

In order to solve the problem of uncertain noise during the measurement of actual system, an extended Kalman filter fusionestimation method based on multisensor fusion algorithm with uncertain effects is proposed. Then the equivalent measurementand the corresponding error matrix are estimated by the proposed uncertain fusing algorithm. Submit the results into the systemmodel for filter processing and the optimal estimation can be obtained by the filtering method. Finally, the algorithm is verified inthe GPS/INS navigation system which shows that the fusion result with uncertainty effect is much better than then fusion resultwith independent noise due to the consideration of correlated noise and uncertain effects for the actual system.This is also validatesthe effectiveness and practicality of the proposed algorithm.

1. Introduction

In order to improve the accuracy of vehicle positioning,multiple sensors are installed on the vehicle to overcomethe defect of single sensor. For instance, global positioningsystem (GPS) is a satellite-based navigation system andcan provide accurate three-dimensional information on theposition and velocity. However, the lack of credibility andnonautonomous of GPS in some cases, often leads to blindfield and cannot be effectively positioned. Inertial navigationsystem (INS) is a standalone system that provides linear accel-eration and angular velocity at high speed, but as time goesby it significantly reduces its accuracy, position, and velocityand error rate information increased. The combination ofGPS and INS can provide accuracy position and enhance thereliability of the system, and therefore has beenwidely appliedin the integrated navigation system [1–3].

The Kalman filtering (KF) approach is one of the widelyused methods for the navigation system [4, 5]. However, KFis the optimal estimation of minimummean square error fora linear time invariant system and has a strict requirementfor the process noise covariance and measurement noisecovariance. Based on theKF, othermethods like particle filter,

cubature Kalman filter (CKF), and extended Kalman particlefilter are also used in navigation [6, 7]. Arasaratnam et al.studied the continuous discrete system by the cubature KF[6]. However the statistical characteristics of noise shouldbe decided when using filter method and therefore manystudies assumed that the measurement noise and systemnoise are independent white noise [8–10]. Cappelle et al.presented an algorithm of vehicle positioning based onKalman filter, which integrated data from INS and GPSwith the assumption that the system noise and measurementnoise are independent white noise [11]. Carlson presented thefamous federated square root filter which assumes the initialestimation error cross-covariance matrices among the localsubsystems to be zero [12]. Deng and Qi proposed a fusionalgorithm weighted by scalars with the assumption that thestate estimation errors among the local subsystems to beuncorrelated and simplify the calculation [13].While the idealassumption, which local estimators are with independent oruncorrelated measurement noise, does not accord with thegeneral case, and sometimes can cause bias and divergenceproblem due to cumulative errors for the actual system.

In order to better describe the actual system, some schol-ars have studied the fusion estimation with the correlated

Hindawi Publishing CorporationMathematical Problems in EngineeringVolume 2015, Article ID 218561, 9 pageshttp://dx.doi.org/10.1155/2015/218561

2 Mathematical Problems in Engineering

noise [14, 15]. Giremus et al. study the correlated noisefor GPS navigation by comparing two KF methods [14].Based on the work [13], Sun studied the correlation amonglocal estimation errors and proposed a multisensor optimalinformation fusion algorithm weighted by matrices in thelinear minimum variance sense [16, 17]. An optimal fusionalgorithm using general weighting matrices for more thantwo local correlated estimates was proposed by Hong et al.[18], where the global state estimate was a linear combinationof all the local state estimates, and this required a largeamount of calculation. To reduce the amount of calculations,Qiu et al. studied a diagonal weighting matrices method forfusion of local estimates [19]; however it introduced a lossin estimation accuracy. The present fusion methods for thenavigation system are mainly focused on the independentnoise and correlated noises anddonot conclude the uncertainfactors during the measurement for the actual system.

In practice, whenGPS/INS and other sensors are installedon the vehicle to navigate, the measurement noise is notonly correlated, but also there exists some uncertainty effects.For instance, the common environment, like the vibrationof the platform (vehicle) during the driving, will causethe measurements noise from all sensors installed on itto be intercorrelated. What is more, the not well-informedstatistical characteristics of measurement noise and thetransformation error of different coordinate systems causinguncertainty factors do exist in the actual system. Sometimes,the uncertainty will significantly affect the accuracy of thefusion estimation. This means that the filter method cannotbe used directly for the actual system when considering theuncertainty factors during the measurement.

This paper presents an extended Kalman filter fusionestimation method based on multisensor fusion algorithmwith uncertainty effect. After dealing with the measurementinformation of multiple sensors, the equivalent measurementis obtained, together with the corresponding error matrix.Using the extended Kalman filtering, the exact position willbe obtained.

The outline of the remainder of the paper is as follows.In Section 2, the system model is given and the extendedKalman filter function is briefly introduced. The uncertaintyfusion estimation algorithm is the main subject of Section 3.In Section 4, results from the field tests are presented toillustrate the effectiveness of the algorithm proposed inSection 3. Section 5 contains a summary and conclusions.

2. System Model

Consider the stochastic system with multiple sensors:

x (𝑡) = f [x (𝑡) , u (𝑡) ,w (𝑡) , 𝑡]

z𝑖 (𝑘) = h

𝑖 (x (𝑘)) + k

𝑖 (𝑘) (𝑖 = 1, 2, . . . , 𝑚) ,

(1)

where, x is the state vector, w is the system noise, z𝑖(𝑘) (𝑖 =

1, 2, . . . , 𝑚) the measurement vector of sensor 𝑖 and k𝑖(𝑘) is

the corresponding noise, f(⋅) and h𝑖(⋅) are the continuous

function, denote the state function and measurement func-tion, and u is the input vector.

Different sensors, installed on the same platform, sharethe same environment which makes the measurements noisek𝑖(𝑘), are intercorrelated, and uncertainty factors exist for

the vibration of the vehicle during the driving. Thereforethe statistics characteristics should be decided before usingextended KF on system (1). After the fusion processing of themeasurement information, the system (1) can be rewritten as

x (𝑡) = f [x (𝑡) ,w (𝑡) , 𝑡]

z (𝑘) = h (x (𝑘)) + k (𝑘) ,

(2)

where, z(𝑘) is the equivalent measurement obtained byuncertain fusion estimation algorithm, and k(𝑘) is the cor-responding error with zero mean and varianceR(𝑘). Discretesystem (2), the extended Kalman filter algorithm is

x (𝑘 | 𝑘) = x (𝑘 | 𝑘 − 1) + K (𝑘) [z (𝑘) − h (x (𝑘 | 𝑘 − 1))]

K (𝑘) = P (𝑘 | 𝑘 − 1)H (𝑘)

𝑇

× [H (𝑘)P(𝑘 | 𝑘 − 1)H (𝑘)

𝑇+ R (𝑘)]

−1

P (𝑘 | 𝑘) = [Ι − K (𝑘)H (𝑘)]P (𝑘 | 𝑘 − 1)

x (𝑘 | 𝑘 − 1) = f (x (𝑘 − 1 | 𝑘 − 1))

P (𝑘 | 𝑘 − 1) = F (𝑘, 𝑘 − 1)P (𝑘 − 1 | 𝑘 − 1) F (𝑘, 𝑘 − 1)

𝑇

+ Q (𝑘 − 1) ,

(3)

where, 𝑘 denote the discrete time, x(𝑘 | 𝑘) is the estimator ofstate vector x at time 𝑘, x(𝑘 | 𝑘−1) is the predictor of x(𝑘−1 |

𝑘 − 1), z(𝑘) is the measurement value at time 𝑘, K(𝑘) denotesthe Kalman gainmatrix, P(𝑘 | 𝑘−1) and P(𝑘 | 𝑘) are theMSEmatrix of x(𝑘 | 𝑘−1) and x(𝑘 | 𝑘), Ι denote the identitymatrix,and F(𝑘, 𝑘 − 1) = 𝜕f/𝜕x|x(𝑘−1|𝑘−1), H(𝑘) = 𝜕h/𝜕x|x(𝑘|𝑘−1), andR(𝑘),Q(𝑘) are the covariance matrix of measurement systemnoise.

3. Uncertain Fusion Estimation Algorithm

Theory. Suppose that a multisensor system consists of 𝑚

sensors, and the system noise is composed of correlatednoise and uncertainty term. The measurement informationobtained from the 𝑖th sensor at time 𝑘 is denoted:

z𝑖 (𝑘) = z (𝑘) + k

𝑖 (𝑘) + Δz

𝑖 (𝑘) 𝑖 = 1, 2, . . . , 𝑚, (4)

where, z(𝑘) ∈ 𝑅

𝑛 denotes physical vector to be estimated,z𝑖(𝑘) ∈ 𝑅

𝑛, (𝑖 = 1, 2, . . . , 𝑚) is the measurement valuefrom 𝑖th sensor at time 𝑘, corresponding measurement erroris k𝑖(𝑘) ∈ 𝑅

𝑛, (𝑖 = 1, 2, . . . , 𝑚) with zero mean and thecross-covariance matrices R

𝑖𝑗(𝑘) ∈ 𝑅

𝑛×𝑛, Δz𝑖(𝑘) ∈ 𝑅

𝑛, and(𝑖 = 1, 2, . . . , 𝑚) is the uncertainty term. Then the fusionestimation result z(𝑘) and corresponding error covariancematrix R(𝑘) are given by the following:

Mathematical Problems in Engineering 3

z (𝑘) =

𝑚

∑

𝑖=1

Λ𝑖 (𝑘) z𝑖 (𝑘) ,

R (𝑘)

=

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘) [𝑎𝑖𝑗

A (𝑘) + 𝑏

𝑖𝑗R𝑖𝑖 (

𝑘) + 𝑐

𝑖𝑗R𝑗𝑗 (

𝑘)]Λ𝑇

𝑗(𝑘) ,

(5)

where z(𝑘) is fusion estimation, R(𝑘) is the correspondingerror covariance matrix, 𝑎

𝑖𝑗> 0, and 𝑏

𝑗𝑖> 0, 𝑐

𝑖𝑗> 0 are

positive real numbers. A(𝑘) = sup𝑖=1,2,...,𝑚

𝐸(Δz𝑖(𝑘)Δz𝑇

𝑖(𝑘)) is

supremum of uncertainty error, 𝐸 is the expectation, and thesuperscript 𝑇 denotes the transpose. Λ

1(𝑘),Λ

2(𝑘), . . . ,Λ

𝑚(𝑘)

are coefficient matrix, which can be calculated by the follow-ing:

[Λ1 (𝑘) Λ2 (𝑘) ⋅ ⋅ ⋅ Λ𝑚−1 (

𝑘)] = W (𝑘) S−1 (𝑘) ,

Λ𝑚 (𝑘) = I −

𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘) ,

(6)

W (𝑘) = [T𝑚𝑚 (𝑘) − T1𝑚 (𝑘) T

𝑚𝑚 (𝑘) − T2𝑚 (𝑘) ⋅ ⋅ ⋅ T

𝑚𝑚 (𝑘) − T𝑚−1,𝑚 (𝑘)] , (7)

S (𝑘)

=

[

[

[

[

[

[

S11 (

𝑘) S12 (

𝑘) ⋅ ⋅ ⋅ S1,𝑚−1 (

𝑘)

S𝑇21

(𝑘) S22 (

𝑘)

.

.

. S2,𝑚−1 (

𝑘)

.

.

.

.

.

. d.

.

.

S𝑇𝑚−1,1

(𝑘) S𝑇𝑚−1,2

(𝑘) ⋅ ⋅ ⋅ S𝑚−1,𝑚−1 (

𝑘)

]

]

]

]

]

]

=

[

[

[

[

[

[

T11 (𝑘) − T1𝑚 (𝑘) − T𝑚1 (𝑘) + T𝑚𝑚 (𝑘) T

12 (𝑘) − T1𝑚 (𝑘) − T𝑚2 (𝑘) + T𝑚𝑚 (𝑘) ⋅ ⋅ ⋅ T1,𝑚−1 (𝑘) − T1𝑚 (𝑘) − T𝑚,𝑚−1 (𝑘) + T𝑚𝑚 (𝑘)

T21 (𝑘) − T2𝑚 (𝑘) − T𝑚1 (𝑘) + T𝑚𝑚 (𝑘) T


.

.

.... d

.

.

.

T𝑚−1,1 (𝑘) − T𝑚−1,𝑚 (𝑘) − T𝑚1 (𝑘) + T𝑚𝑚 (𝑘) T

𝑚−1,1 (𝑘) − T𝑚−1,𝑚 (𝑘) − T𝑚2 (𝑘) + T𝑚𝑚 (𝑘) ⋅ ⋅ ⋅ T𝑚−1,𝑚−1 (𝑘) − T𝑚−1,𝑚 (𝑘) − T𝑚,𝑚−1 (𝑘) + T𝑚𝑚 (𝑘)

]

]

]

]

]

]

(8)T𝑖𝑗 (

𝑘) = 𝑎

𝑖𝑗A (𝑘) + 𝑏

𝑖𝑗R𝑖𝑖 (

𝑘) + 𝑐

𝑖𝑗R𝑗𝑗 (

𝑘) , (9)

where R𝑖𝑗(𝑘) (𝑖, 𝑗 = 1, 2, . . . , 𝑚) are the cross-covariance of

k𝑖(𝑘), k𝑗(𝑘) (𝑖, 𝑗 = 1, 2, . . . , 𝑚) and the value of 𝑎

𝑖𝑗, 𝑏𝑖𝑗, 𝑐𝑖𝑗and

A(𝑘) are determined by the statistical characteristics of theactual system and the accuracy of the sensors.

Proof. Let z(𝑘) ∈ 𝑅

𝑛 is the physical vector to be estimated;z𝑖(𝑘) ∈ 𝑅

𝑛(𝑖 = 1, 2, . . . , 𝑚) denote observation value from

the 𝑖th sensor at time 𝑘, k𝑖(𝑘) ∈ 𝑅

𝑛, (𝑖 = 1, 2, . . . , 𝑚)

is the corresponding measurement error, which is assumedto be zero mean, covariance matrix is R

𝑖𝑖(𝑘) ∈ 𝑅

𝑛×𝑛, andΔz𝑖(𝑘) ∈ 𝑅

𝑛 denotes uncertainty term from the 𝑖th sensor;the measurement can be written as

z𝑖 (𝑘) = z (𝑘) + k

𝑖 (𝑘) + Δz

𝑖 (𝑘) 𝑖 = 1, 2, . . . , 𝑚. (10)

Define the fusion estimator as

z (𝑘) =

𝑚

∑

𝑖=1

Λ𝑖 (𝑘) z𝑖 (𝑘) , (11)

where, z(𝑘) denotes fusion result and Λ1(𝑘),Λ

2(𝑘), . . .,

Λ𝑚(𝑘) ∈ 𝑅

𝑛×𝑛 are weighting matrices to be determined.It is well known that a global optimal estimate should beunbiased and the error covariance matrix is minimum. From

the assumption of an unbiased condition, the constraints ofthe respective weights should satisfy

𝑚

∑

𝑖=1

Λ𝑖 (𝑘) = I. (12)

Therefore the error covariance matrix is

R (𝑘) = 𝐸

[

[

(

𝑚

∑

𝑖=1

Λ𝑖 (𝑘) (k𝑖 (𝑘) + Δz

𝑖 (𝑘)))

× (

𝑚

∑

𝑗=1

Λ𝑗 (

𝑘) (k𝑗 (𝑘) + Δz𝑗 (

𝑘)))

𝑇

]

]

= 𝐸(

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘) Δz𝑖 (𝑘) Δz

𝑇

𝑗(𝑘)Λ𝑇

𝑗(𝑘)

+

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘) Δz𝑖 (𝑘) k

𝑇

𝑗(𝑘)Λ𝑇

𝑗(𝑘)

+

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘) k𝑖 (𝑘) Δz

𝑇

𝑗(𝑘)Λ𝑇

𝑗(𝑘)

+

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘) k𝑖 (𝑘) k

𝑇

𝑗(𝑘)Λ𝑇

𝑗(𝑘))


=

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘) [𝐸 (Δz

𝑖 (𝑘) Δz𝑇𝑗(𝑘))

+ 𝐸 (Δz𝑖 (𝑘) k𝑇𝑗(𝑘))

+ 𝐸 (k𝑖 (𝑘) Δz𝑇𝑗(𝑘))

+𝐸 (k𝑖 (𝑘) k𝑇𝑗(𝑘))]Λ

𝑇

𝑗(𝑘) .

(13)

For any real number 𝜆

𝑖𝑗= 0, 𝜆𝑗𝑖

= 0, and 𝜇

𝑖𝑗= 0, the

following inequality is true:

𝐸[(𝜆

𝑖𝑗Δz𝑖 (𝑘) −

k𝑗 (

𝑘)

𝜆

𝑖𝑗

)(𝜆


k𝑗 (

𝑘)

𝜆

𝑖𝑗

)

𝑇

] ≥ 0,

𝐸 [(𝜆

𝑗𝑖Δz𝑗 (

𝑘) −

k𝑖 (𝑘)

𝜆

𝑗𝑖

)(𝜆

𝑗𝑖Δz𝑗 (

𝑘) −

k𝑖 (𝑘)

𝜆

𝑗𝑖

)

𝑇

] ≥ 0,

𝐸 [(𝜇


Δz𝑗 (

𝑘)

𝜇

𝑖𝑗

)(𝜇


Δz𝑗 (

𝑘)

𝜇

𝑖𝑗

)

𝑇

] ≥ 0.

(14)

Simplify the above equation, then

𝐸 (Δz𝑖 (𝑘) k𝑇𝑗(𝑘)) + 𝐸 (k

𝑗 (𝑘) Δz𝑇𝑖(𝑘))

≤ 𝜆

2

𝑖𝑗𝐸 (Δz

𝑖 (𝑘) Δz𝑇𝑖(𝑘)) +

𝐸 (k𝑗 (

𝑘) k𝑇𝑗(𝑘))

𝜆

𝑖𝑗

= 𝜆

2

𝑖𝑗𝐸 (Δz


R𝑗𝑗 (

𝑘)

𝜆

2

𝑖𝑗

,

𝐸 (Δz𝑗 (

𝑘) k𝑇𝑖(𝑘)) + 𝐸 (k

𝑖 (𝑘) Δz𝑇𝑗(𝑘))

≤ 𝜆

2

𝑗𝑖𝐸 (Δz

𝑗 (𝑘) Δz𝑇𝑗(𝑘)) +

𝐸 (k𝑖 (𝑘) k𝑇𝑖(𝑘))

𝜆

2

𝑗𝑖

= 𝜆

2

𝑗𝑖𝐸 (Δz

𝑗 (𝑘) Δz𝑇𝑗(𝑘)) +

R𝑖𝑖 (

𝑘)

𝜆

2

𝑗𝑖

,

𝐸 (Δz𝑖 (𝑘) Δz𝑇𝑗(𝑘)) + 𝐸 (Δz

𝑗 (𝑘) Δz𝑇𝑖(𝑘))

≤ 𝜇

2

𝑖𝑗𝐸 (Δz


𝐸 (Δz𝑗 (

𝑘) Δz𝑇𝑗(𝑘))

𝜇

2

𝑖𝑗

.

(15)

Define A(𝑘) = sup𝑖=1,2,...,𝑚

𝐸(Δz𝑖(𝑘)Δz𝑇

𝑖(𝑘)) and denote

the supremum of the uncertainty error. Combine (15), then

𝐸 (Δz𝑖 (𝑘) Δz𝑇𝑗(𝑘)) + 𝐸 (Δz

𝑖 (𝑘) k𝑇𝑗(𝑘))

+ 𝐸 (k𝑖 (𝑘) Δz𝑇𝑗(𝑘)) + 𝐸 (k

𝑖 (𝑘) k𝑇𝑗(𝑘))

≤

𝜆

2

𝑖𝑗+ 𝜇

2

𝑖𝑗

2 ⋅ 𝐸 (Δz𝑖 (𝑘) Δz𝑇𝑖(𝑘))

+

R𝑖𝑖 (

𝑘)

2𝜆

2

𝑗𝑖

+

R𝑗𝑗 (

𝑘)

2𝜆

2

𝑖𝑗

+

𝜆

2

𝑗𝑖+ 1/𝜇

2

𝑖𝑗

2 ⋅ 𝐸 (Δz𝑗 (

𝑘) Δz𝑇𝑗(𝑘))

.

(16)

Let 𝑎

𝑖𝑗= (𝜆

2

𝑖𝑗+ 𝜆

2

𝑗𝑖+ 𝜇

2

𝑖𝑗+ 1/𝜇

2

𝑖𝑗)/2, 𝑏

𝑖𝑗= 1/(2𝜆

2

𝑗𝑖),

𝑐

𝑖𝑗= 1/(2𝜆

2

𝑖𝑗), and T

𝑖𝑗(𝑘) = 𝑎

𝑖𝑗A(𝑘) + 𝑏

𝑖𝑗R𝑖𝑖(𝑘) + 𝑐

𝑖𝑗R𝑗𝑗(𝑘),

It is obvious that

𝑎

𝑖𝑗> 0, 𝑏

𝑗𝑖> 0, 𝑐

𝑖𝑗> 0

R (𝑘) ≤

𝑚

∑

𝑖=1

𝑚

∑

𝑗=1

Λ𝑖 (𝑘)T𝑖𝑗 (𝑘)Λ

𝑇

𝑗(𝑘) .

(17)

Let P(𝑘) = ∑

𝑚

𝑖=1∑

𝑚

𝑗=1Λ𝑖(𝑘)T𝑖𝑗(𝑘)Λ𝑇

𝑗(𝑘), then

P (𝑘) =

𝑚−1

∑

𝑖=1

𝑚−1

∑

𝑗=1

Λ𝑖 (𝑘)T𝑖𝑗 (𝑘)Λ

𝑇

𝑗(𝑘)

+

𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘)T𝑖𝑚 (𝑘) [I −

𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘)]

𝑇

+

𝑚−1

∑

𝑖=1

[I −𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘)]T

𝑚𝑗 (𝑘)Λ𝑇

𝑗(𝑘)

+ [I −𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘)]T

𝑚𝑚 (𝑘) [I −𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘)]

𝑇

.

(18)

Let 𝜕 tr(P(𝑘))/𝜕Λ𝑘(𝑘) = 0 (𝑘 = 1, 2, . . . , 𝑚 − 1), (tr

denotes trace of the matrix), then

𝑚−1

∑

𝑖=1

Λ𝑖 (𝑘) [T𝑖𝑙 (𝑘) − T

𝑖𝑚 (𝑘) − T𝑚𝑙 (

𝑘) + T𝑚𝑚 (𝑘)]

= T𝑚𝑚 (𝑘) − T

𝑙𝑚 (𝑘) (𝑙 = 1, 2, . . . , 𝑚 − 1) .

(19)

The matrix form of (19) can be written as

[Λ1 (𝑘) Λ2 (𝑘) ⋅ ⋅ ⋅ Λ𝑚−1 (

𝑘)] = W (𝑘) S−1 (𝑘) , (20)

where


W (𝑘) = [T𝑚𝑚 (𝑘) − T1𝑚 (𝑘) T

𝑚𝑚 (𝑘) − T2𝑚 (𝑘) ⋅ ⋅ ⋅ T

𝑚𝑚 (𝑘) − T𝑚−1,𝑚 (𝑘)] ,

S (𝑘)

=

[

[

[

[

[

[

S11 (

𝑘) S12 (

𝑘) ⋅ ⋅ ⋅ S1,𝑚−1 (

𝑘)

S𝑇21

(𝑘) S22 (

𝑘)

.

.

. S2,𝑚−1 (

𝑘)

.

.

.

.

.

. d.

.

.

S𝑇𝑚−1,1

(𝑘) S𝑇𝑚−1,2

(𝑘) ⋅ ⋅ ⋅ S𝑚−1,𝑚−1 (

𝑘)

]

]

]

]

]

]

=

[

[

[

[

[

[

T11 (𝑘) − T1𝑚 (𝑘) − T𝑚1 (𝑘) + T𝑚𝑚 (𝑘) T


T21 (𝑘) − T2𝑚 (𝑘) − T𝑚1 (𝑘) + T𝑚𝑚 (𝑘) T


.

.

.... d

.

.

.

T𝑚−1,1 (𝑘) − T𝑚−1,𝑚 (𝑘) − T𝑚1 (𝑘) + T𝑚𝑚 (𝑘) T

𝑚−1,1 (𝑘) − T𝑚−1,𝑚 (𝑘) − T𝑚2 (𝑘) + T𝑚𝑚 (𝑘) ⋅ ⋅ ⋅ T𝑚−1,𝑚−1 (𝑘) − T𝑚−1,𝑚 (𝑘) − T𝑚,𝑚−1 (𝑘) + T𝑚𝑚 (𝑘)

]

]

]

]

]

]

.

(21)

Then weighting matrices Λ1(𝑘),Λ

2(𝑘), . . . ,Λ

𝑚−1(𝑘) ∈ 𝑅

𝑛×𝑛

can be determined by solving (20) and Λ𝑚(𝑘) can be deter-

mined by Λ𝑚(𝑘) = I − ∑

𝑚−1

𝑖=1Λ𝑖(𝑘).

Note. By the processing of the proof, it can be seen that 𝑏𝑖𝑗

=

𝑐

𝑗𝑖, 𝑎𝑖𝑗

≥ 1 + 1/(4𝑏

𝑖𝑗) + 1/(4𝑐

𝑖𝑗), and the values of parameters

𝑎

𝑖𝑗, 𝑏𝑖𝑗, and 𝑐

𝑖𝑗can be decided from any real numbers 𝜆

𝑖𝑗and

𝜇

𝑖𝑗as shown in (15). For simplicity, the values can be selected

as 𝜆𝑖𝑗

= 1, 𝜇𝑖𝑗

= 1. Therefore, 𝑎𝑖𝑗

= 2, 𝑏𝑖𝑗

= 1/2, 𝑐𝑖𝑗

= 1/2.Considering the covariance of fusion result should be less

than the covariance of any single measurement, the followingis true: R(𝑘) ≤ R

𝑖𝑗(𝑘); that is,

𝑎

𝑖𝑗A (𝑘) + 𝑏

𝑖𝑗R𝑖𝑖 (

𝑘) + 𝑐

𝑖𝑗R𝑗𝑗 (

𝑘) ≤ R𝑖𝑗 (

𝑘) . (22)

Therefore, the parameters in the theory should satisfy

𝑎

𝑖𝑗≥ 1 +

1

4𝑏

𝑖𝑗

+

1

4𝑐

𝑖𝑗

,

A (𝑘) ≤

[R𝑖𝑗 (

𝑘) − 𝑏

𝑖𝑗R𝑖𝑖 (

𝑘) − 𝑐

𝑖𝑗R𝑗𝑗 (

𝑘)]

𝑎

𝑖𝑗

.

(23)

4. Algorithm Validation andExperiment Analysis

To illustrate the effectiveness of the algorithm, the vehiclesystem installed two GPS, INS, and other sensors, and thenorth-east-day geographic coordinates is selected as thenavigation coordinate system. The system error equationsand measuring equations are

x (𝑡) = F (𝑡) x (𝑡) + G (𝑡)w (𝑡) ,

z (𝑡) = H (𝑡) x (𝑡) + k (𝑡) ,

(24)

where, x = [𝜙

𝐸, 𝜙

𝑁, 𝜙

𝑈, 𝛿𝑉

𝐸, 𝛿𝑉

𝑁, 𝛿𝑉

𝑈, 𝛿𝐿, 𝛿𝜆, 𝛿ℎ, 𝜀

𝐸, 𝜀

𝑁,

𝜀

𝑈, ∇

𝐸, ∇

𝑁, ∇

𝑈]

𝑇, 𝜙𝐸, 𝜙𝑁, and 𝜙

𝑈are the misalignment angle

for three direction; 𝛿𝑉𝑁, 𝛿𝑉𝐸, and 𝛿𝑉

𝑈are the east velocity,

north velocity, and up velocity error, 𝛿𝐿, 𝛿𝜆, 𝛿ℎ are thelatitude, longitude, and altitude error, and 𝜀

𝐸, 𝜀𝑁, and 𝜀

𝑈are

the drift of the east, north, and up gyroscope. ∇𝐸, ∇𝑁, and

∇

𝑈are the bias of accelerometer, F(𝑡) is the system transfer

matrix, z = [𝑉

𝐸 GPS −𝑉

𝐸 INS, 𝑉𝑁 GPS −𝑉

𝑁 INS, 𝑉𝑈 GPS −𝑉

𝑈 INS,𝜆GPS − 𝜆INS, 𝐿GPS − 𝐿 INS, ℎGPS − ℎINS]

𝑇, G(𝑡) is the systemnoise matrix, w(𝑡) is the Gauss noise with zero mean,H(𝑡) = [0

3×3, I6×6

, 06×6

] is themeasurementmatrix, k(𝑡) is themeasurement noise, 𝜆GPS, 𝐿GPS, ℎGPS, 𝜆INS, 𝐿 INS, and ℎINS arethe position measured by GPS and INS, and 𝑉

𝐸 GPS, 𝑉𝑁 GPS,𝑉

𝑈 GPS,𝑉𝐸 INS,𝑉𝑁 INS, and𝑉

𝑈 INS are the velocitymeasured byGPS and INS.The state equation is expressed as the following.

(1) Attitude error functions

𝜙

𝐸= 𝜙

𝑁[𝜔

𝑖𝑒sin 𝐿 +

𝑉

𝐸tan 𝐿

𝑅

𝑁+ ℎ

]

− 𝜙

𝑈[𝜔

𝑖𝑒cos 𝐿 +

𝑉

𝐸

𝑅

𝑀+ ℎ

]

−

𝛿𝑉

𝑁

𝑅

𝑀+ ℎ

+

𝑉

𝑁𝛿ℎ

[(𝑅

𝑀+ ℎ)

2]

+ 𝜀

𝐸,

𝜙

𝑁= − 𝜙

𝐸[𝜔

𝑖𝑒sin 𝐿 +

𝑉

𝐸tan𝐿

𝑅

𝑁+ ℎ

]

−

𝜙

𝑈𝑉

𝑁

𝑅

𝑀+ ℎ

+

𝛿𝑉

𝐸

𝑅

𝑁+ ℎ

− 𝜔

𝑖𝑒sin 𝐿𝛿𝐿 −

𝑉

𝐸𝛿ℎ

[(𝑅

𝑁+ ℎ)

2]

+ 𝜀

𝑁,


𝜙

𝑈= 𝜙

𝐸[𝜔

𝑖𝑒cos 𝐿 +

𝑉

𝐸

𝑅

𝑁+ ℎ

] +

𝜙

𝑁𝑉

𝑁

𝑅

𝑀+ ℎ

+

tan𝐿𝛿𝑉

𝐸

𝑅

𝑁+ ℎ

−

𝑉

𝐸tan 𝐿𝛿ℎ

[(𝑅

𝑁+ ℎ)

2]

+ [𝜔

𝑖𝑒cos 𝐿 +

𝑉

𝐸sec2𝐿

𝑅

𝑁+ ℎ

] 𝛿𝐿 + 𝜀

𝑈.

(25)

(2) Velocity error functions

𝛿

𝑉

𝐸= − 𝑓

𝑈𝜙

𝑁+ 𝑓

𝑁𝜙

𝑈+ [

𝑉

𝑁tan𝐿 − 𝑉

𝑈

𝑅

𝑁+ ℎ

] 𝛿𝑉

𝐸

+ [2𝜔

𝑖𝑒sin 𝐿 +

𝑉

𝐸tan𝐿

𝑅

𝑁+ ℎ

] 𝛿𝑉

𝑁

− [2𝜔

𝑖𝑒cos 𝐿 +

𝑉

𝐸

𝑅

𝑁+ ℎ

] 𝛿𝑉

𝑈

+ [2𝜔

𝑖𝑒(𝑉

𝑈sin 𝐿 + 𝑉

𝑁cos 𝐿) +

𝑉

𝐸𝑉

𝑁sec2𝐿

𝑅

𝑁+ ℎ

] 𝛿𝐿

+ [

𝑉

𝐸𝑉

𝑈− 𝑉

𝐸𝑉

𝑁tan𝐿

(𝑅

𝑁+ ℎ)

2]𝛿ℎ + ∇

𝐸,

𝛿

𝑉

𝑁= − 𝑓

𝐸𝜙

𝑈+ 𝑓

𝑈𝜙

𝐸

− [2𝜔

𝑖𝑒sin 𝐿 +

𝑉

𝐸tan𝐿

𝑅

𝑁+ ℎ

] 𝛿𝑉

𝐸

− [

𝑉

𝑈

𝑅

𝑀+ ℎ

] 𝛿𝑉

𝑁− [

𝑉

𝑁

𝑅

𝑀+ ℎ

] 𝛿𝑉

𝑈

− [2𝜔

𝑖𝑒𝑉

𝐸cos 𝐿 +

𝑉

2

𝐸sec2𝐿

𝑅

𝑁+ ℎ

] 𝛿𝐿

+ [

𝑉

𝑈𝑉

𝑁

(𝑅

𝑀+ ℎ)

2+

𝑉

𝐸tan𝐿

(𝑅

𝑁+ ℎ)

2]𝛿ℎ + ∇

𝑁,

𝛿

𝑉

𝑈= 𝑓

𝐸𝜙

𝑁− 𝑓

𝑁𝜙

𝐸

+ [2𝜔

𝑖𝑒cos 𝐿 +

𝑉

𝐸

𝑅

𝑁+ ℎ

] 𝛿𝑉

𝐸

+ [

2𝑉

𝑁

𝑅

𝑀+ ℎ

] 𝛿𝑉

𝑁− 2𝜔

𝑖𝑒𝑉

𝐸sin 𝐿𝛿𝐿

− [

𝑉

𝑁

(𝑅

𝑀+ ℎ)

2+

𝑉

𝐸

(𝑅

𝑁+ ℎ)

2]𝛿ℎ + ∇

𝑈.

(26)

(3) Position error functions

𝛿

𝐿 = [

1

𝑅

𝑀+ ℎ

] 𝛿𝑉

𝑁− [

𝑉

𝑁

(𝑅

𝑀+ ℎ)

2]𝛿ℎ

𝛿

𝜆 = [

sec 𝐿𝑅

𝑁+ ℎ

] 𝛿𝑉

𝐸

+ [

𝑉

𝐸sec 𝐿 tan𝐿

𝑅

𝑁+ ℎ

] 𝛿𝐿

− [

𝑉

𝐸sec 𝐿

(𝑅

𝑁+ ℎ)

2]𝛿ℎ

𝛿

ℎ = 𝛿𝑉

𝑈.

(27)

(4) Error functions of inertial device

𝜀

𝐸= 0, 𝜀

𝑁= 0, 𝜀

𝑈= 0,

∇

𝐸= 0,

∇

𝑁= 0,

∇

𝑈= 0,

(28)

where 𝑅

𝑀= 𝑅(1 − 2𝑒 + 3𝑒 sin2𝐿), 𝑅

𝑁= 𝑅(1 + 𝑒 sin2𝐿)

are the radius of curvature for the meridian plane and theprime vertical plane.𝑅 is the length ofmajor semiaxis, 𝑒 is theeccentricity ratio of the earth, the subscripts 𝐸, 𝑁, 𝑈 denotethe directions of earth, north, andup,𝐿,𝜆,ℎdenote longitude,latitude, and altitude, 𝜔

𝑖𝑒= 15.04107∘/h is the rotational

velocity in the earth inertial space, and 𝑔 = 9.8m/s2 is thegravitational acceleration.

Shown in Figure 1 is themeasurement information of atti-tude and acceleration of the vehicle during the experiment.

Without loss of generality, the measurement noise ofdifferent sensors from the vehicle system is affected byuncertainty term.

According to formula (24), the parameters in the theoryare selected as 𝑎

𝑖𝑗= 2, 𝑏

𝑖𝑗= 1/2, 𝑐

𝑖𝑗= 1/2, and A(𝑘) =

0.5(R𝑖𝑗(𝑘)−0.5R

𝑖𝑖(𝑘)−0.5R

𝑗𝑗(𝑘)), whereR

𝑖𝑗(𝑘) are determined

by the accuracy of the sensors. The equivalent measurementvalue, obtained by formula (6), is substituted into the systemequations (24) and then the longitude, latitude, and altitudeof the vehicle will be obtained after the filtering. The resultsare shown in Figures 2, 3, and 4.

The above figures show that the estimated value oflongitude and latitude is matched with the measured valueverywell, while the difference of the altitude is relatively large.This is due to the distance from north-south direction onthe sphere changing 111 kilometers when the latitude changesone degree. That is to say, the distance will change 30.9meters when latitude changes one second. The proportionof transfer relationship for two units for longitude is similarwith latitude, while the proportion for longitudinal is not theconstant; it is also related with latitude of the position, but thebasic proportions will not vary too much. Therefore, whenthe unit of longitude and latitude is degrees, the differencebetween estimated and measured value is very small.

In order to illustrate uncertainty effects on the fusionerror of longitude, latitude, and altitude, Figures 5, 6, and 7


0 120 240 360 480 600

0 120 240 360 480 6000

10

20

0 120 240 360 480 600

0 120 240 360 480 6000

200

400

Time (s)0 120 240 360 480 600

Time (s)

Time (s)

Time (s)Time (s)

Time (s)

0 120 240 360 480 600−5

0

5

Roll

angl

e (∘)

Pitc

h an

gle (

∘)

Yaw

angl

e (∘)

−0.5

0.5

0

−0.5

1

0.5

0

−0.5

−1

−1.5

ax

(g)

az

(g)

ay

(g)

Figure 1: Response of attitude and acceleration information of the vehicle.

0 60 120 180 240 300 360 420 480 540 600108.752

108.754

108.756

108.758

108.76

108.762

108.764

108.766

Time (s)MeasuredEstimated

126 128

108.7534108.7534108.7534

Long

itude

(∘)

Figure 2: Measured and estimated value of the longitude.

show the fusion error curves of vehicle position, whose unitis meters.

From the introduction part, it can be seen that manyfusion algorithms are studied with the assumption that themeasurement noises are independent with Gaussian noisedistribution. In order to compare the results of this paperwiththe independent noise assumption, the fusion error curveswith independent noise assumption are also shown in Figures5 to 7.

From Figures 5 to 7, it can be seen that for the samemeasurement information, the fusion error of longitude,altitude, and altitude with uncertainty noise assumption is

0 60 120 180 240 300 360 420 480 540 60034

34.005

34.01

34.015

34.02

34.025

34.03

34.035

Time (s)

12813013234.01

34.0102

MeasuredEstimated

Latit

ude (

∘)

Figure 3: Measured and estimated value of the latitude.

better than the independent noise assumption. It can bealso observed that the difference of altitude error curves ismuch more obvious than the longitude and latitude errorswith two assumptions. This is because that the changes inlongitude and latitude are mainly affected by the longitudinaland lateral movement of the vehicle, and change of altitudeis mainly affected by the vertical movement of the vehicle.Because the uncertainty factors in the experiment are mainlyfrom the vehicle vibration during the driving, the effects ofvibration to longitudinal and lateral motion is less than thevertical motion. This conclusion illustrates that the uncer-tainty factors do really exist for the actual system especially


0 60 120 180 240 300 360 420 480 540 600440

450

460

470

480

490

500

510

Time (s)

Alti

tude

(m)

125 130 135 140 145

472

473

MeasuredEstimated

Figure 4: Measured and estimated value of the latitude.

0 60 120 180 240 300 360 420 480 540 600Time (s)

Long

itude

erro

r (m

)

Independent noiseUncertainty noise

−15

−10

−5

0

5

10

15

20

Figure 5: Longitude error with independent and uncertainty noise.

when multiple sensors are installed on the same platform.The vibration of platform makes the measurement noisesof different sensors are intercorrelated and the statisticalcharacteristic of the noise is unknown. Taking into accountthe uncertainty factors of measurement noise in the actualsystem can help to improve the positioning accuracy of thevehicle.

5. Conclusions

For the actual system with multiple sensors to detect thesame state vector, this paper proposed an extended Kalmanfilter method based on multisensor fusion algorithm withuncertainty disturbance. This method can deal with theproblem of uncertainty effects of measurement noise byfusing the measurement information and filtering the systemequation.

What is more, the accurate fusion results can be obtainedjust by one time filtering which reduces the calculation. Thefusion result is verified by the GPS/INS navigation system,which shows that the algorithm proposed here consideringthe uncertainty influences for the system enhances the posi-tioning accuracy for actual system.

0 60 120 180 240 300 360 420 480 540 600Time (s)

Latit

ude e

rror

(m)

−15

−10

−5

0

5

10

15


Figure 6: Latitude error with independent and uncertainty noise.

0 60 120 180 240 300 360 420 480 540 6000

5

10

15

20

25

Time (s)

Alti

tude

erro

r (m

)


Figure 7: Altitude error with independent and uncertainty noise.

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper.

Acknowledgments

This research is supported by the National Natural ScienceFoundation of China (no. 50974103) and Educational Com-mission of Shaanxi Province of China (2013JK0858).

References

[1] D.-J. Jwo and S.-H. Wang, “Adaptive fuzzy strong trackingextended Kalman filtering for GPS navigation,” IEEE SensorsJournal, vol. 7, no. 5, pp. 778–789, 2007.

[2] A. Noureldin, A. El-Shafie, and M. Bayoumi, “GPS/INS inte-gration utilizing dynamic neural networks for vehicular navi-gation,” Information Fusion, vol. 12, no. 1, pp. 48–57, 2011.

[3] G. A. Einicke, G. Falco, and J. T. Malos, “Bounded constrainedfiltering for GPS/INS integration,” IEEE Transactions on Auto-matic Control, vol. 58, no. 1, pp. 125–133, 2013.

[4] J. Rezaie, B. Moshiri, B. N. Araabi, and A. Asadian, “GPS/INSintegration using nonlinear blending filters,” in Proceedings of


the SICE Annual Conference, pp. 1674–1680, IEEE, Takamatsu,Japan, September 2007.

[5] F. Caron, E. Duflos, D. Pomorski, and P. Vanheeghe, “GPS/IMUdata fusion using multisensor Kalman filtering: introduction ofcontextual aspects,” Information Fusion, vol. 7, no. 2, pp. 221–230, 2006.

[6] I. Arasaratnam, S. Haykin, and T. R. Hurd, “Cubature Kalmanfiltering for continuous-discrete systems: theory and simula-tions,” IEEE Transactions on Signal Processing, vol. 58, no. 10,pp. 4977–4993, 2010.

[7] M. Simandl and J. Dunık, “Derivative-free estimation methods:new results and performance analysis,” Automatica, vol. 45, no.7, pp. 1749–1757, 2009.

[8] Q. Gan and J. H. Chris, “Comparison of two measure-ment fusion methods for kalman-filter-based multisensor datafusion,” IEEE Transactions on Aerospace and Electronic Systems,vol. 37, no. 1, pp. 273–280, 2001.

[9] J. Liu and B. Cai, “Adaptive algorithm of information fusion forintegrated navigation,” Journal of Beijing JiaotongUniversity, vol.32, no. 2, pp. 88–93, 2008.

[10] Z.-L. Deng and Y. Gao, “Fast information fusion Kalman filter,”Control and Decision, vol. 20, no. 1, pp. 27–31, 2005.

[11] C. Cappelle, D. Pomorski, andY. Yang, “GPS/INS data fusion forland vehicle localization,” inProceedings of the IMACSMulticon-ference on Computational Engineering in Systems Applications,vol. 1, pp. 21–27, Beijing, China, 2006.

[12] N. A. Carlson, “Federated square root filter for decentralizedparallel processes,” IEEE Transactions on Aerospace and Elec-tronic Systems, vol. 26, no. 3, pp. 517–525, 1990.

[13] Z. Deng and R. Qi, “Multi-sensor information fusion subopti-mal steady-state Kalman filter,” Chinese Science Abstracts, vol. 6,no. 2, pp. 183–184, 2000.

[14] A. Giremus, E. Grivel, and F. Castanie, “Is𝐻∞filtering relevant

for correlated noises in GPS navigation?” in Proceedings of the16th International Conference on Digital Signal Processing (DSP’09), pp. 1–6, IEEE, July 2009.

[15] X. Wang, Y. Liang, Q. Pan, and F. Yang, “A Gaussian approx-imation recursive filter for nonlinear systems with correlatednoises,” Automatica, vol. 48, no. 9, pp. 2290–2297, 2012.

[16] S.-L. Sun, “Multi-sensor optimal information fusion Kalmanfilters with applications,” Aerospace Science and Technology, vol.8, no. 1, pp. 57–62, 2004.

[17] S. Sun and Z. Deng, “Multi-sensor optimal information fusionKalman filter,” Automatica, vol. 40, no. 6, pp. 1017–1023, 2004.

[18] J. Hong, Z. Hong, and Y. Hong, “Fusion algorithm of correlatedlocal estimates for federated filter,” in Proceedings of the 3rdAsian Control Conference, pp. 1428–1433, Shanghai, China,2000.

[19] H. Z. Qiu, H. Y. Zhang, and H. Jin, “Fusion algorithm ofcorrelated local estimates,” Aerospace Science and Technology,vol. 8, no. 7, pp. 619–626, 2004.

Submit your manuscripts athttp://www.hindawi.com

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

MathematicsJournal of


Mathematical Problems in Engineering

Hindawi Publishing Corporationhttp://www.hindawi.com

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of


Probability and StatisticsHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

Journal of


Mathematical PhysicsAdvances in

Complex AnalysisJournal of


OptimizationJournal of


CombinatoricsHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

International Journal of


Operations ResearchAdvances in

Journal of


Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

International Journal of Mathematics and Mathematical Sciences


The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Algebra

Discrete Dynamics in Nature and Society



Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com

Volume 2014 Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

Stochastic AnalysisInternational Journal of

Documents

Research Article Fusion Estimation Algorithm with Uncertain Noises and Its Application ... · 2019. 5. 2. · Research Article Fusion Estimation Algorithm with Uncertain Noises and