# Fusion Estimation Algorithm with Uncertain Noises and Its Application in Navigation System

Authors: Zhiping Ren; Huili Wang
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

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


---

## Body

## 1. Introduction

In order to improve the accuracy of vehicle positioning, multiple sensors are installed on the vehicle to overcome the defect of single sensor. For instance, global positioning system (GPS) is a satellite-based navigation system and can provide accurate three-dimensional information on the position and velocity. However, the lack of credibility and nonautonomous of GPS in some cases, often leads to blind field and cannot be effectively positioned. Inertial navigation system (INS) is a standalone system that provides linear acceleration and angular velocity at high speed, but as time goes by it significantly reduces its accuracy, position, and velocity and error rate information increased. The combination of GPS and INS can provide accuracy position and enhance the reliability of the system, and therefore has been widely applied in the integrated navigation system [1–3].The Kalman filtering (KF) approach is one of the widely used methods for the navigation system [4, 5]. However, KF is the optimal estimation of minimum mean square error for a linear time invariant system and has a strict requirement for the process noise covariance and measurement noise covariance. Based on the KF, other methods like particle filter, cubature Kalman filter (CKF), and extended Kalman particle filter 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 should be decided when using filter method and therefore many studies assumed that the measurement noise and system noise are independent white noise [8–10]. Cappelle et al. presented an algorithm of vehicle positioning based on Kalman filter, which integrated data from INS and GPS with the assumption that the system noise and measurement noise are independent white noise [11]. Carlson presented the famous federated square root filter which assumes the initial estimation error cross-covariance matrices among the local subsystems to be zero [12]. Deng and Qi proposed a fusion algorithm weighted by scalars with the assumption that the state estimation errors among the local subsystems to be uncorrelated and simplify the calculation [13]. While the ideal assumption, which local estimators are with independent or uncorrelated measurement noise, does not accord with the general case, and sometimes can cause bias and divergence problem due to cumulative errors for the actual system.In order to better describe the actual system, some scholars have studied the fusion estimation with the correlated noise [14, 15]. Giremus et al. study the correlated noise for GPS navigation by comparing two KF methods [14]. Based on the work [13], Sun studied the correlation among local estimation errors and proposed a multisensor optimal information fusion algorithm weighted by matrices in the linear minimum variance sense [16, 17]. An optimal fusion algorithm using general weighting matrices for more than two local correlated estimates was proposed by Hong et al. [18], where the global state estimate was a linear combination of all the local state estimates, and this required a large amount of calculation. To reduce the amount of calculations, Qiu et al. studied a diagonal weighting matrices method for fusion of local estimates [19]; however it introduced a loss in estimation accuracy. The present fusion methods for the navigation system are mainly focused on the independent noise and correlated noises and do not conclude the uncertain factors during the measurement for the actual system.In practice, when GPS/INS and other sensors are installed on the vehicle to navigate, the measurement noise is not only correlated, but also there exists some uncertainty effects. For instance, the common environment, like the vibration of the platform (vehicle) during the driving, will cause the measurements noise from all sensors installed on it to be intercorrelated. What is more, the not well-informed statistical characteristics of measurement noise and the transformation error of different coordinate systems causing uncertainty factors do exist in the actual system. Sometimes, the uncertainty will significantly affect the accuracy of the fusion estimation. This means that the filter method cannot be used directly for the actual system when considering the uncertainty factors during the measurement.This paper presents an extended Kalman filter fusion estimation method based on multisensor fusion algorithm with uncertainty effect. After dealing with the measurement information of multiple sensors, the equivalent measurement is obtained, together with the corresponding error matrix. Using the extended Kalman filtering, the exact position will be obtained.The outline of the remainder of the paper is as follows. In Section2, the system model is given and the extended Kalman filter function is briefly introduced. The uncertainty fusion estimation algorithm is the main subject of Section 3. In Section 4, results from the field tests are presented to illustrate the effectiveness of the algorithm proposed in Section 3. Section 5 contains a summary and conclusions.


## 2. System Model

Consider the stochastic system with multiple sensors:(1)x˙(t)=f[x(t),u(t),w(t),t]zik=hixk+viki=1,2,…,m,
where, x is the state vector, w is the system noise, zi(k)(i=1,2,…,m) the measurement vector of sensor i and vi(k) is the corresponding noise, f(·) and hi(·) are the continuous function, denote the state function and measurement function, and u is the input vector.Different sensors, installed on the same platform, share the same environment which makes the measurements noisevi(k), are intercorrelated, and uncertainty factors exist for the vibration of the vehicle during the driving. Therefore the statistics characteristics should be decided before using extended KF on system (1). After the fusion processing of the measurement information, the system (1) can be rewritten as
(2)x˙(t)=f[x(t),w(t),t]z(k)=h(x(k))+v(k),
where, z(k) is the equivalent measurement obtained by uncertain fusion estimation algorithm, and v(k) is the corresponding error with zero mean and variance R(k). Discrete system (2), the extended Kalman filter algorithm is
(3)x^(k∣k)=x^(k∣k-1)+K(k)[z(k)-h(x^(k∣k-1))]Kk=P(k∣k-1)HkT×HkP(k∣k-1)HkT+Rk-1P(k∣k)=[Ι-K(k)H(k)]P(k∣k-1)x^(k∣k-1)=f(x^(k-1∣k-1))P(k∣k-1)=F(k,k-1)P(k-1∣k-1)Fk,k-1T+Qk-1,
where, k denote the discrete time, x^(k∣k) is the estimator of state vector x at time k, x^(k∣k-1) is the predictor of x^(k-1∣k-1), z(k) is the measurement value at time k, K(k) denotes the Kalman gain matrix, P(k∣k-1) and P(k∣k) are the MSE matrix of x^(k∣k-1) and x^(k∣k), Ι denote the identity matrix, and F(k,k-1)=∂f/∂xx^(k-1∣k-1), H(k)=∂h/∂xx^(k∣k-1), and R(k), Q(k) are the covariance matrix of measurement system noise.


## 3. Uncertain Fusion Estimation Algorithm

Theory. Suppose that a multisensor system consists of m sensors, and the system noise is composed of correlated noise and uncertainty term. The measurement information obtained from the ith sensor at time k is denoted:
(4)zik=zk+vik+Δziki=1,2,…,m,
where, z(k)∈Rn denotes physical vector to be estimated, zi(k)∈Rn,  (i=1,2,…,m) is the measurement value from ith sensor at time k, corresponding measurement error is vi(k)∈Rn, (i=1,2,…,m) with zero mean and the cross-covariance matrices Rij(k)∈Rn×n, Δzi(k)∈Rn, and (i=1,2,…,m) is the uncertainty term. Then the fusion estimation result z^(k) and corresponding error covariance matrix R(k) are given by the following:
(5)z^k=∑i=1mΛikzik,R(k)=∑i=1m∑j=1mΛikaijAk+bijRiik+cijRjjkΛjTk,
where z^(k) is fusion estimation, R(k) is the corresponding error covariance matrix, aij>0, and bji>0, cij>0 are positive real numbers. A(k)=supi=1,2,…,mE(Δzi(k)ΔziT(k)) is supremum of uncertainty error, E is the expectation, and the superscript T denotes the transpose. Λ1k,Λ2k,…,Λm(k) are coefficient matrix, which can be calculated by the following:(6)Λ1(k)Λ2(k)⋯Λm-1(k)=WkS-1k,Λmk=I-∑i=1m-1Λik,(7)W(k)=Tmm(k)-T1m(k)Tmm(k)-T2m(k)⋯Tmm(k)-Tm-1,m(k),(8)Sk=S11(k)S12(k)⋯S1,m-1(k)S21T(k)S22(k)⋮S2,m-1(k)⋮⋮⋱⋮Sm-1,1T(k)Sm-1,2T(k)⋯Sm-1,m-1(k)=T11(k)-T1m(k)-Tm1(k)+Tmm(k)T12(k)-T1m(k)-Tm2(k)+Tmm(k)⋯T1,m-1(k)-T1m(k)-Tm,m-1k+TmmkT21(k)-T2m(k)-Tm1(k)+Tmm(k)T22(k)-T2m(k)-Tm2(k)+Tmm(k)⋯T2,m-1(k)-T2m(k)-Tm,m-1k+Tmmk⋮⋮⋱⋮Tm-1,1(k)-Tm-1,m(k)-Tm1(k)+Tmm(k)Tm-1,1(k)-Tm-1,m(k)-Tm2(k)+Tmm(k)⋯Tm-1,m-1(k)-Tm-1,m(k)-Tm,m-1k+Tmmk(9)Tijk=aijAk+bijRiik+cijRjjk,where Rij(k)(i,j=1,2,…,m) are the cross-covariance of vi(k), vj(k)(i,j=1,2,…,m) and the value of aij, bij, cij and A(k) are determined by the statistical characteristics of the actual system and the accuracy of the sensors.Proof.
Letz(k)∈Rn is the physical vector to be estimated; zi(k)∈Rn(i=1,2,…,m) denote observation value from the ith sensor at time k, vi(k)∈Rn, (i=1,2,…,m) is the corresponding measurement error, which is assumed to be zero mean, covariance matrix is Rii(k)∈Rn×n, and Δzi(k)∈Rn denotes uncertainty term from the ith sensor; the measurement can be written as
(10)zik=zk+vik+Δziki=1,2,…,m.
Define the fusion estimator as(11)z^k=∑i=1mΛikzik,
where, z^(k) denotes fusion result and Λ1k,Λ2k,…,Λm(k)∈Rn×n are weighting matrices to be determined. It is well known that a global optimal estimate should be unbiased and the error covariance matrix is minimum. From the assumption of an unbiased condition, the constraints of the respective weights should satisfy
(12)∑i=1mΛi(k)=I.
Therefore the error covariance matrix is
(13)Rk=E∑i=1mΛikvik+Δzik∑j=1mΛjkvjk+ΔzjkThhh×∑j=1mΛjkvjk+ΔzjkT=E∑i=1m∑j=1mΛi(k)Δzi(k)ΔzjT(k)ΛjT(k)hhh+∑i=1m∑j=1mΛi(k)Δzi(k)vjT(k)ΛjT(k)hhh+∑i=1m∑j=1mΛikvikΔzjTkΛjT(k)hhh+∑i=1m∑j=1mΛi(k)vi(k)vjT(k)ΛjT(k)=∑i=1m∑j=1mΛikEΔzikΔzjTkhhhhhhhhhh+EΔzikvjTkhhhhhhhhhh+EvikΔzjTkhhhhhhhhhh+EvikvjTkΛjT(k).
For any real numberλij≠0, λji≠0, and μij≠0, the following inequality is true:
(14)EλijΔzik-vjkλijλijΔzik-vjkλijT≥0,EλjiΔzjk-vikλjiλjiΔzjk-vikλjiT≥0,EμijΔzik-ΔzjkμijμijΔzik-ΔzjkμijT≥0.
Simplify the above equation, then(15)E(Δzi(k)vjT(k))+E(vj(k)ΔziT(k))≤λij2E(Δzi(k)ΔziT(k))+EvjkvjTkλij=λij2E(Δzi(k)ΔziT(k))+Rjjkλij2,E(Δzj(k)viT(k))+E(vi(k)ΔzjT(k))≤λji2E(Δzj(k)ΔzjT(k))+EvikviTkλji2=λji2E(Δzj(k)ΔzjT(k))+Riikλji2,E(Δzi(k)ΔzjT(k))+E(Δzj(k)ΔziT(k))≤μij2E(Δzi(k)ΔziT(k))+EΔzjkΔzjTkμij2.
DefineA(k)=supi=1,2,…,mE(Δzi(k)ΔziT(k)) and denote the supremum of the uncertainty error. Combine (15), then
(16)E(Δzi(k)ΔzjT(k))+E(Δzi(k)vjT(k))+E(vi(k)ΔzjT(k))+E(vi(k)vjT(k))≤λij2+μij22·EΔzikΔziTk+Riik2λji2+Rjjk2λij2+λji2+1/μij22·E(Δzj(k)ΔzjT(k)).
Letaij=(λij2+λji2+μij2+1/μij2)/2, bij=1/(2λji2), cij=1/(2λij2), and Tij(k)=aijA(k)+bijRii(k)+cijRjj(k), It is obvious that
(17)aij>0,bji>0,cij>0Rk≤∑i=1m∑j=1mΛikTijkΛjTk.
LetP(k)=∑i=1m∑j=1mΛi(k)Tij(k)ΛjT(k), then
(18)Pk=∑i=1m-1∑j=1m-1Λi(k)Tij(k)ΛjT(k)+∑i=1m-1Λi(k)Tim(k)I-∑i=1m-1ΛikT+∑i=1m-1I-∑i=1m-1ΛikTmj(k)ΛjT(k)+I-∑i=1m-1ΛikTmmkI-∑i=1m-1ΛikT.
Let∂tr⁡(P(k))/∂Λk(k)=0  (k=1,2,…,m-1), (tr⁡ denotes trace of the matrix), then
(19)∑i=1m-1Λik[Til(k)-Tim(k)-Tml(k)+Tmm(k)]=Tmmk-Tlmkl=1,2,…,m-1.
The matrix form of (19) can be written as
(20)Λ1kΛ2k⋯Λm-1k=WkS-1k,
where(21)Wk=Tmm(k)-T1m(k)Tmm(k)-T2m(k)⋯Tmm(k)-Tm-1,m(k),Sk=S11(k)S12(k)⋯S1,m-1(k)S21T(k)S22(k)⋮S2,m-1(k)⋮⋮⋱⋮Sm-1,1T(k)Sm-1,2T(k)⋯Sm-1,m-1(k)=T11k-T1mk-Tm1k+TmmkT12k-T1mk-Tm2k+Tmmk⋯T1,m-1k-T1mk-Tm,m-1k+TmmkT21k-T2mk-Tm1k+TmmkT22k-T2mk-Tm2k+Tmmk⋯T2,m-1k-T2mk-Tm,m-1k+Tmmk⋮⋮⋱⋮Tm-1,1k-Tm-1,mk-Tm1k+TmmkTm-1,1k-Tm-1,mk-Tm2k+Tmmk⋯Tm-1,m-1k-Tm-1,mk-Tm,m-1k+Tmmk.Then weighting matrices Λ1k,Λ2k,…,Λm-1(k)∈Rn×n can be determined by solving (20) and Λm(k) can be determined by Λm(k)=I-∑i=1m-1Λi(k).Note. By the processing of the proof, it can be seen that bij=cji, aij≥1+1/(4bij)+1/(4cij), and the values of parameters aij, bij, and cij can be decided from any real numbers λij and μij as shown in (15). For simplicity, the values can be selected as λij=1, μij=1. Therefore, aij=2, bij=1/2, cij=1/2.Considering the covariance of fusion result should be less than the covariance of any single measurement, the following is true:R(k)≤Rij(k); that is,
(22)aijAk+bijRiik+cijRjjk≤Rijk.Therefore, the parameters in the theory should satisfy(23)aij≥1+14bij+14cij,Ak≤Rijk-bijRiik-cijRjjkaij.


## 4. Algorithm Validation and Experiment Analysis

To illustrate the effectiveness of the algorithm, the vehicle system installed two GPS, INS, and other sensors, and the north-east-day geographic coordinates is selected as the navigation coordinate system. The system error equations and measuring equations are(24)x˙t=Ftxt+Gtwt,zt=Htxt+vt,
where, x=[ϕE,ϕN,ϕU,δVE,δVN,δVU,δL,δλ,δh,εE,εN,εU,∇E,∇N,∇U]T, ϕE, ϕN, and ϕU are the misalignment angle for three direction; δVN, δVE, and δVU are the east velocity, north velocity, and up velocity error, δL, δλ, δh are the latitude, longitude, and altitude error, and εE, εN, and εU are the drift of the east, north, and up gyroscope. ∇E, ∇N, and ∇U are the bias of accelerometer, F(t) is the system transfer matrix, z=[VE_GPS-VE_INS,VN_GPS-VN_INS,VU_GPS-VU_INS,λGPS-λINS,LGPS-LINS,hGPS-hINS]T, G(t) is the system noise matrix, w(t) is the Gauss noise with zero mean, H(t)=[03×3,I6×6,06×6] is the measurement matrix, v(t) is the measurement noise, λGPS, LGPS, hGPS, λINS, LINS, and hINS are the position measured by GPS and INS, and VE_GPS, VN_GPS, VU_GPS, VE_INS, VN_INS, and VU_INS are the velocity measured by GPS and INS. The state equation is expressed as the following.(1)
Attitude error functions(25)ϕ˙E=ϕNωiesin⁡L+VEtan⁡LRN+h-ϕUωiecos⁡⁡L+VERM+h-δVNRM+h+VNδhRM+h2+εE,ϕ˙N=-ϕEωiesin⁡L+VEtan⁡LRN+h-ϕUVNRM+h+δVERN+h-ωiesin⁡LδL-VEδhRN+h2+εN,ϕ˙U=ϕEωiecos⁡⁡L+VERN+h+ϕNVNRM+h+tan⁡LδVERN+h-VEtan⁡Lδh[RN+h2]+ωiecos⁡⁡L+VEsecL2RN+hδL+εU.(2)
Velocity error functions(26)δV˙E=-fUϕN+fNϕU+VNtan⁡L-VURN+hδVE+2ωiesin⁡L+VEtan⁡LRN+hδVN-2ωiecos⁡⁡L+VERN+hδVU+2ωieVUsin⁡L+VNcos⁡⁡L+VEVNsec2LRN+hδL+VEVU-VEVNtan⁡LRN+h2δh+∇E,δV˙N=-fEϕU+fUϕE-2ωiesin⁡L+VEtan⁡LRN+hδVE-VURM+hδVN-VNRM+hδVU-2ωieVEcos⁡⁡L+VE2sec2LRN+hδL+VUVNRM+h2+VEtan⁡LRN+h2δh+∇N,δV˙U=fEϕN-fNϕE+2ωiecos⁡⁡L+VERN+hδVE+2VNRM+hδVN-2ωieVEsin⁡LδL-VNRM+h2+VERN+h2δh+∇U.(3)
Position error functions(27)δL˙=1RM+hδVN-VNRM+h2δhδλ˙=sec⁡LRN+hδVE+VEsec⁡Ltan⁡LRN+hδL-VEsec⁡LRN+h2δhδh˙=δVU.(4)
Error functions of inertial device(28)ε˙E=0,ε˙N=0,ε˙U=0,∇˙E=0,∇˙N=0,∇˙U=0,where RM=R(1-2e+3esin2L), RN=R(1+esin2L) are the radius of curvature for the meridian plane and the prime vertical plane. R is the length of major semiaxis, e is the eccentricity ratio of the earth, the subscripts E, N, U denote the directions of earth, north, and up, L, λ, h denote longitude, latitude, and altitude, ωie= 15.04107°/h is the rotational velocity in the earth inertial space, and g=9.8 m/s2 is the gravitational acceleration.Shown in Figure1 is the measurement information of attitude and acceleration of the vehicle during the experiment.Figure 1
Response of attitude and acceleration information of the vehicle.Without loss of generality, the measurement noise of different sensors from the vehicle system is affected by uncertainty term.According to formula (24), the parameters in the theory are selected as aij=2, bij=1/2, cij=1/2, and A(k)=0.5(Rij(k)-0.5Rii(k)-0.5Rjj(k)), where Rij(k) are determined by the accuracy of the sensors. The equivalent measurement value, obtained by formula (6), is substituted into the system equations (24) and then the longitude, latitude, and altitude of the vehicle will be obtained after the filtering. The results are shown in Figures 2, 3, and 4.Figure 2
Measured and estimated value of the longitude.Figure 3
Measured and estimated value of the latitude.Figure 4
Measured and estimated value of the latitude.The above figures show that the estimated value of longitude and latitude is matched with the measured value very well, while the difference of the altitude is relatively large. This is due to the distance from north-south direction on the sphere changing 111 kilometers when the latitude changes one degree. That is to say, the distance will change 30.9 meters when latitude changes one second. The proportion of transfer relationship for two units for longitude is similar with latitude, while the proportion for longitudinal is not the constant; it is also related with latitude of the position, but the basic proportions will not vary too much. Therefore, when the unit of longitude and latitude is degrees, the difference between estimated and measured value is very small.In order to illustrate uncertainty effects on the fusion error of longitude, latitude, and altitude, Figures5, 6, and 7 show the fusion error curves of vehicle position, whose unit is meters.Figure 5
Longitude error with independent and uncertainty noise.Figure 6
Latitude error with independent and uncertainty noise.Figure 7
Altitude error with independent and uncertainty noise.From the introduction part, it can be seen that many fusion algorithms are studied with the assumption that the measurement noises are independent with Gaussian noise distribution. In order to compare the results of this paper with the independent noise assumption, the fusion error curves with independent noise assumption are also shown in Figures5 to 7.From Figures5 to 7, it can be seen that for the same measurement information, the fusion error of longitude, altitude, and altitude with uncertainty noise assumption is better than the independent noise assumption. It can be also observed that the difference of altitude error curves is much more obvious than the longitude and latitude errors with two assumptions. This is because that the changes in longitude and latitude are mainly affected by the longitudinal and lateral movement of the vehicle, and change of altitude is mainly affected by the vertical movement of the vehicle. Because the uncertainty factors in the experiment are mainly from the vehicle vibration during the driving, the effects of vibration to longitudinal and lateral motion is less than the vertical motion. This conclusion illustrates that the uncertainty factors do really exist for the actual system especially when multiple sensors are installed on the same platform. The vibration of platform makes the measurement noises of different sensors are intercorrelated and the statistical characteristic of the noise is unknown. Taking into account the uncertainty factors of measurement noise in the actual system can help to improve the positioning accuracy of the vehicle.


## 5. Conclusions

For the actual system with multiple sensors to detect the same state vector, this paper proposed an extended Kalman filter method based on multisensor fusion algorithm with uncertainty disturbance. This method can deal with the problem of uncertainty effects of measurement noise by fusing the measurement information and filtering the system equation.What is more, the accurate fusion results can be obtained just by one time filtering which reduces the calculation. The fusion result is verified by the GPS/INS navigation system, which shows that the algorithm proposed here considering the uncertainty influences for the system enhances the positioning accuracy for actual system.



---

*Source: 218561-2015-01-22.xml*