A Multifilter Location Optimization Algorithm Based On .

Journal of Sensorsdˆi,kNLOSjudgment3NLOSFCMIMMINPUTXi (k)highUKF(high)Xi (k)medUKF(medium)Xi (k)lowUKF(low)ˆ (k ionfittingand FCMˆ (k k)XiLOSKFKFparametercorrectionˆxkˆykBP ��kUKFparametercorrectionR (k 1 ), Q (k 1) for the nextFigure 1: Algorithm flow chart.Set the number of observations to be prepared, fix the realaction track of the mobile node (which does not know thespecific coordinates when measuring), and mark as ðX k , Y k Þat time k. Then, the accurate distance between it and the beacon node isDik ffiffiffiffiffiffiffiffiffiffiffiffiðX i X k Þ2 ðY i Y k Þ2 ,ð2Þwhere in Dit , k represents the kth moment and i representsthe ith beacon node.In the LOS environment, due to the nonideal channel,the actual measured distance through the TOA/TDOA/RSSImodel isDik ðLOSÞ Dik nLOS ,ð3Þwhere nLOS is a Gaussian distribution which has a mean of 0and a standard deviation σLOS , noted as nLOS Nð0, σ2LOS Þ. Itowns the following distribution function. 1n2f ðnLOS Þ pffiffiffiffiffiffiexp LOS:2σ2LOS2π σLOSð4ÞIn the NLOS environment, due to the existence of possible obstacles, which may block the LOS path travelingalong a straight line and compel the signals to travel alonga non-line-of-sight path in the form of refraction or reflection, the actual measured distance becomes more complexjust as [27, 28]Dik ðNLOSÞ Dik nLOS nNLOS ,ð5Þwhere nNLOS has many possibilities, and here it is brieflysummarized as one of Gaussian distribution, uniform distribution, and Poisson distribution.If nNLOS satisfies the Gaussian distribution, it will obeythe Gaussian distribution owning the mean of μNLOS andthe standard deviation of σNLOS , noted as nNLOS NðμNLOS ,σ2NLOS Þ. And its distribution function is as follows:1expf ðnNLOS Þ pffiffiffiffiffiffi2π σNLOS!ðnNLOS μNLOS Þ2: 2σ2NLOSð6ÞWhen nNLOS satisfies a uniform distribution, its minimumand maximum values are a and b, respectively, i.e., nNLOS Uða, bÞ, whose distribution function satisfies as follows:8 1 , a nNLOS b,ð7Þf ðnNLOS Þ b a:0,else:When nNLOS satisfies an exponential distribution, its rateparameter is expressed as λ, i.e., nNLOS EðλÞ, whose distribution function satisfies as follows:(1 exp ð λnNLOS Þ, nNLOS 0,ð8Þf ðnNLOS Þ 0, nNLOS 0:3.2. General Concept. In this paper, a new algorithm named asthe Neural Network Modified Multiple Filter Localization(NNMML) will be put forward. Its algorithm flow is illustratedin Figure 1. At first, the NLOS judgment is carried out towardsthe estimated distance d̂ k . The residual calculation method isadopted here, and the mean value of distance residual isobtained by calculating maximum likelihood estimation coordinates. This method has high practicability and high confidence interval, which can maintain an error rate of less than5% in a high LOS/NLOS mixed environment.After grouping, we perform targeted filtering on thedata. For the LOS case, the updated results are worked outby traditional Kalman Filtering. In the linear environment,the effect of the traditional Kalman filter is always quite satisfactory. For the NLOS case, due to the large difference of

4Journal of SensorsNLOS errors, the measurement data will be classified byNLOS classification based on FCM. High NLOS measurements, medium ones, and soft ones will be processed afterFCM. Then, the NLOS measurements after filtering can beobtained by the interacting multimodel (IMM).After we get the updated results, the noise parameterssuch as the covariance matrix are adjusted adaptively byusing corresponding formulae for KF and UKF. The filteritself is constantly used to judge whether the dynamic ofthe system has changed and updates the noise parametersso as to be used in the next filtering. This adaptive approachcan improve the robustness of the algorithm in complexenvironments.Then, the distance data of multiple nodes were integrated to obtain the preliminary positioning results by theleast square method, and the curve was simply fitted according to multiple moment data. FCM was used to remove thepreliminary positioning results seriously deviating from theoverall trend. Finally, the BP neural network is used to takethe preliminary positioning results that fit the overall trendas the training set. Specifically, we use gradient descent tocomplete the back propagation of network parameters.Eventually, the output after training fills the gap of the positioning results, and we get the full result after correction.As is shown in formula (10), if ωk,i is zero, it is judged inthe LOS case, otherwise in the NOLS environment.3.3. NNMML AlgorithmΔt is the time interval between two measurements and wðkÞrepresents the process noise vector.Moreover, measurement equation is obtained as follows:3.3.1. NLOS Judgment Based on Residual Calculation. Underthe condition that the error value brought by NLOS isunknown, N estimated distance values fd̂ i,k i 1, 2, Ngfrom N (N 3) beacon nodes can be obtained at a certainmeasurement time k. According to the permutation andcombination, we can have group M distance estimation.Through the basic Newtonian least square method, them-pair maximum likelihood estimation coordinates of thê k,m , Ŷ k,m Þ m 1, 2, 3 Mg calculatedmobile node fðXfrom the above m-set data can be obtained. Then, thê k,m , Ŷ k,m Þ and the coordinatedistance between each group ðXof beacon node ðX i , Y i Þ is figured out, and the averageresidual difference is calculated with the estimated distanceas follows:M:The calculated residual difference between the ith beaconnode and the mobile node that we achieve at time k willmake a comparison with the measurement noise standarddeviation σ to confirm whether it belongs to the NLOS category or not.ωk,i 0, ϵ k,i σ,1, ϵ k,i σ:ð10Þð11Þwhere d̂ i,k represents the distance between the ith beaconnode and the mobile node and d i,k denotes the speed ofmobile node.Thus, state equation can be worked out as follows:X i ðkÞ FX i ðk 1Þ CwðkÞ,ð12Þwhere coefficient matrices are, respectively,"F 103Δt 2, C 4 2 5:1ΔtΔt2#Z i ðkÞ HX i ðkÞ vðkÞ,ð13Þð14Þwhere column vectorH " #10,ð15Þand vðkÞ delegates the measurement noise vector.From the above state equation and measurement equation, the following iterative formula of the KF algorithmcan be derived:Pi ðk k 1Þ FPi ðk 1 k 1Þ F T Rðk 1Þ,ð9Þ(hiX i ðkÞ d̂ i,k , d i,k ,̂ i ðk k 1 Þ F X̂ i ðk 1 k 1Þ,Xs ��ffiffi �ffiffiffiffiffiffiffi 2ffiMd i,k m 1ðX k,m X i Þ2 ðY k,m Y i Þ2ϵ k ,i 3.3.2. Kalman Filtering. Since the KF algorithm still has satisfactory accuracy in the linear environment, Kalman Filtering is still adopted in the LOS environment.Firstly, we define the basic state parameters betweenthe ith beacon node and the mobile node at time k as X i ðkÞ:ð16Þwhere RðkÞ represents covariance matrix of the measurednoise whose initial value is set as σ2LOS CC T .̂ i ðk k 1Þ,E i ðk Þ Z i ðk Þ H XSi ðkÞ HPi ðk k 1ÞH T Qðk 1Þ,ð17Þð18Þwhere QðkÞ represents the covariance matrix of the observednoise which is independent of RðkÞ.From following formula (19), the incremental Kalmangain K i ðkÞ in the iteration is ciphered out:K i ðkÞ Pi ðk k 1ÞH T ðSi ðkÞÞ 1 :ð19Þ

Journal of Sensors5The updated state parameters and covariance requiredby the round of iteration can be obtained from the Kalmangain K i ðkÞ as follows:̂ i ðk kÞ X̂ i ðk k 1 Þ E i ðk ÞK i ðk Þ,XPi ðk kÞ Pi ðk k 1ÞðI K i ðkÞH Þ,2n̂ i ðk k 1Þ ωmj X i, j ðk 1Þ,Xj 02n ̂ i ðk k 1ÞPi ðk k 1Þ ωcj X i, j ðk 1Þ Xð20Þj 0 T X i, j ðk 1Þ X i ðk k 1Þ QðkÞ,2n̂ ðk k 1Þ ωmj Z i, j ðk 1Þ,Zwhere I is the identity matrix.j 03.3.3. Unscented Kalman Filtering. Unscented Kalman Filtering (UKF) is an improved algorithm of Kalman Filtering.Although its advantages are not obvious in the LOS environment with linear signal propagation, it can significantly correct the nonlinear error and improve the positioningaccuracy in the nonlinear environment. So UKF is adoptedin the NLOS environment in this paper.(1) Initialization. Its state equation and measurement equation can be derived from the same formulae in KF. Therefore, those formulae will not be repeated and elaborated inthis part.ð22Þwhere RðkÞ and QðkÞ stand for covariance matrix of measurement noise and covariance matrix of observationnoise, respectively.(4) State Update. On the basis of the formulae of Part 3, theupdated variance and covariance matrix can be achieved asfollows:2n ̂ i ðk k 1ÞPxxi ðk k 1Þ ωcj X i, j ðk 1Þ Xj 0For the convenience of subsequent calculation, X i isdefined as the basic state parameters between the ith beaconnode and the mobile node. xi and Pi are, respectively, itsmathematical expectation and its covariance matrix.(2) Calculate the Sigma Points and Their Weights. Such astate, as an N-dimensional random variate, owns a total of2n 1 sigma points, which are obtained by formula as follows: T X i, j ðk 1Þ X i ðk k 1Þ QðkÞ,2n ̂ i ðk k 1ÞPzz i ðk k 1Þ ωcj Z i, j ðk 1Þ Zj 0 T Z i, j ðk 1Þ Z i ðk k 1Þ RðkÞ,2n ̂ i ðk k 1ÞPxz i ðk k 1Þ ωcj X i, j ðk 1Þ Xj 08 xi ðk 1Þ j 0, ffiffiffi X i, j ðk 1Þ xi ðk 1Þ ðn λÞðPi ðk 1ÞÞ j j 1, 2, 3, 4, n, ffiffiffiffiffiffiffi : x ðk 1Þ ðn λÞðP ðk 1ÞÞj n 1, , 2n,iij nð21Þwhere ðPi ðk 1ÞÞ j signified the jth column in the covariancematrix Pi .The sigma point weights follow the following law ωm0 mλ/ðn λÞ, ω j 1/2ðn λÞ, j 1, 2, 3, , 2n.And the weights of its variances satisfy ωc0 λ/ðn λÞ ð1 α2 βÞ, ωcj 1/2ðn λÞ, j 1, 2, 3, , 2n, where λfollows the expression λ α2 ðn kÞ n, α is determined bythe dispersion degrees of the above sigma points, and theoptimal value of β that can be obtained in Gaussian distribution is two. In addition, in this circumstance, k is set as zero.(3) State Prediction. From the state equation and measurement equation, the following state prediction of the UKFalgorithm can be derived: T Z i, j ðk 1Þ Z i ðk k 1Þ :ð23ÞIn the wake, the Kalman gain of UKF can be figured out:K i ðkÞ Pxz i ðk k 1ÞPzz 1i ðk k 1Þ:ð24ÞIn light of the above Kalman gain, the updated stateparameter and its covariance will be done as follows:̂ i ðk k 1Þ ,̂ i ðk k 1Þ K i ðkÞ Z i ðkÞ Ẑ i ðk Þ XXPi ðk kÞ Pi ðk k 1Þ K i ðkÞPzz i ðk k 1ÞK Ti ðkÞ Pi ðk k 1Þ K i ðkÞPxz i ðk k 1Þ:ð25ÞAt this point, one of its iterations ends.3.3.4. Fuzzy C-Means Clustering. In practice, the distributionof NLOS is more complex, which will lead to great uncertainty in the error parameters of NLOS. In order to alleviatethis problem, the FCM method is adopted for the NLOS

6Journal of Sensorsgroup after NLOS judgment, and then, filtering is carried outafter classification.At time k, the distance between the M beacon nodes and edðkÞ ½de ðkÞ, , df ðkÞ .the mobile node is denoted as dðkÞ,1MLet each element be divided into c groups; ½c1 , c2 , , cc is the cluster center matrix, and μij represents the membership degree of the jth distance element to the ith group, thatis, the degree to which it belongs to this group. In this paper,c is set to 3.Then, the objective function and constraint conditions ofFCM are obtained as follows:cMi 1 j 1s:t: : j 1, 2, , M,p11pg1where dij represents the Euclidean distance between the ithcluster center and the jth distance element and M indicatesthe fuzzy weight factor.In order to make JðU, c1 , c2 , , cc Þ reach the minimumvalue, the updated objective function is obtained by theLagrange multiplier method:LðU, c1 , c2 , , cc , λ1 , λ2 , , λM ÞMcj 1i 1! J ðU, c1 , c2 , , cc Þ λ j μij 1 ,m Mj 1 μij d i ðkÞm Mj 1 μijpij μi ðk 1Þ, i pij μi ðk 1Þð30Þ p1g1C CA, pgg ð31Þand parameter g in the matrix before is the dimensionnumber.Then, the measurement estimates can be weightedthrough the mixing probabilities. At the same time, itscovariance is recalculated: j ðk 1 k 1 Þ X̂ i ðk 1 k 1Þμi, j ðk 1 k 1Þ,Xið27Þwhere λ is the Lagrange multiplier.The clustering center and the membership degree of itscorresponding elements can be obtained through partialderivative calculation:ð32Þh j ðk 1 k 1Þ Pi ðk 1 k 1Þ X̂ i ðk 1 k 1 ÞPî ij ðk 1 k 1ÞÞ X̂ i ðk 1 k 1Þ XiT̂ ij ðk 1 k 1Þ μi, j ðk 1 k 1Þ: Xð33Þ,i ½1, c ,1 ck 10Bp B@ μij ½0, 1 ,μij μi, j ðk 1 k 1Þ ð26Þi 1ci (1) Interactive Input. In the first step, the mixing probabilitycan be figured out, whose values are the ratio of the initialprobability after the weighted transfer probability to its normalized coefficient:where pij represents the transition probability which obeysthe Markov Transition Probability Matrix2J ðU, c1 , c2 , , cc Þ μmij d ij8 c μij 1,Owing to NLOS cases, the state equation and measurement equation satisfy formula (22) in the UKF without further elaboration.dij /d kj2/ðm 1Þ:ð28Þð29ÞBy membership degree, M distance elements are dividedinto the group with the largest membership degree. Therefore, when c 3 here, they can be divided into high NLOSmeasurements, medium ones, and low ones.3.3.5. Interactive Multimodel Algorithm. The InteractingMultimodel (IMM) algorithm is an algorithm that is basedon the Bayesian theory, through multiple filters to achievethe purpose of model adaptation. There are four steps inthe IMM algorithm: interactive input, filter use, probabilityupdate, and interactive output. In this paper, the algorithmis used to carry out the grouping calculation under NLOScases in order to achieve a more robust state estimation.(2) Filter Use. The state parameters and covariance obtainedby Equations (32) and (33) were used as the input of thefilter, and UKF was selected as the filter in the NLOS environment. By the formulae in Section 3.3.3, the correspond̂ j ðk kÞ, P j ðk kÞ, E j ðkÞ, S j ðkÞ.ing results can be calculated: X(3) Probability Update. In this part, the probability is redistributed for the next iteration, and the updated mixing probability is as follows:μ j ðk Þ L j ðkÞ i pij μi ðk 1Þ j L j ðkÞ i pij μi ðk 1Þ,ð34Þwhere the L j ðkÞ above represents the maximum likelihoodfunction on measurement equation, and it is a function ofresidual E j ðkÞ as the dependent variable, which is followingformula (35).

Journal of Sensors71L j ðkÞ qffiffiffiffiffiffiffiffiffiffi exp2π S j ðkÞ!E2j ðkÞ N E j ðkÞ ; 0, S j ðkÞ :2S j ðkÞð35Þ(4) Interactive Output. The mixing probability obtained in(34) is used as the weight to finally get the state parametersand covariance.̂ ðk k Þ μ j ðk Þ X̂ j ðk k Þ,XH " #1"F 0,1Δt01ð40Þ#:(2) Parameter Correction Formulae Applied to UKF. For thesake of simplifying the formal expression of the UKF revision, we define it for the time being that̂ i ðk k 1Þ:Ei ðkÞ Z i ðkÞ Zĵ j ð k kÞ X̂ ðk kÞ,E j ðk Þ XhiPðk kÞ μ j ðkÞ P j ðk kÞ E j ðkÞETj ðkÞ :ð36ÞThe modification of the measurement noise covariancematrix RðkÞ in the iteration is as follows: Rðk 1Þ ω k Ei ðkÞETi ðkÞ ðPzz i ðk k 1Þ RðkÞÞj ð1 ω k ÞRðkÞ:3.3.6. Parameter Correction. In order to improve the overalladaptive performance of the algorithm, NNMML carries outan additional error correction for the observation noisecovariance matrix QðkÞ and the measurement noise covariance matrix RðkÞ after each filtering iteration, so as to reducethe error caused by the initial covariance value in the iteration process and improve the accuracy of filtering. The relevant parameters and formulae will be briefly describedbelow.Let the correction constant be b, which does not changein a given location. Then, we can reach the correction weightby the following formula:ω k 1 b1 bk 1:(1) Parameter Correction Formulae Applied to KF. The modification of the measurement noise covariance matrix RðkÞ inthe iteration is as follows:Rðk 1Þ ωk Ei ðkÞETi ðkÞ HPi ðk k 1ÞHð42ÞThe modification of the observation noise covariancematrix QðkÞ in the iteration is as follows: Qðk 1Þ ω k K i ðkÞEi ðkÞETi ðkÞK Ti ðkÞ Pi ðk kÞ ðPxxi ðk k 1Þ QðkÞ ð1 ω k ÞQðkÞ:ð43ÞFor fear of the loss of positive quality in the iteration ofthe above noise covariance matrix, the positive qualityshould be tested after correcting the parameters in eachround. If the positive nature is lost, this round of parametercorrection will be abandoned.ð37ÞThe modified formulae for KF and UKF filteringmethods are slightly different, which will be brieflydescribed below. ð41Þ T ð1 ω k ÞRðkÞ:ð38ÞThe modification of the observation noise covariancematrix QðkÞ in the iteration is as follows: Qðk 1Þ ω k K i ðkÞEi ðkÞETi ðkÞK Ti ðkÞ Pi ðk kÞ FPi ðk 1 k 1Þ F T ð1 ω k ÞQðkÞ,ð39Þwhere matrices H and F are the same as described in KFfiltering that3.3.7. Preliminary Location Estimation. Through the abovê i ðk kÞ, which combines the results ofsubalgorithm, the Xthe two filtering methods, can be obtained; namely, we getthe revised distance d̂ i,k between ith beacon node (coordinates ðX i , Y i Þ) and mobile node at every measured time.If the coordinates of the mobile node at the time k aredefined as ðxk , yk Þ, an underdetermined system of equationsof it is able to be listed as follows:82 ðxk X 1 Þ2 ðyk Y 1 Þ2 d̂ 1,k , 2 ðxk X 2 Þ2 ðyk Y 2 Þ2 d̂ 2,k , 2 ðxk X 3 Þ2 ðyk Y 3 Þ2 d̂ 3,k , 2:ðxk X n Þ2 ðyk Y n Þ2 d̂ n,k :ð44ÞBy simple transposition and matrix operations, it iscapable of being rewritten as the following matrix equation:½A ½X ½b ,ð45Þ

8Journal of Sensorswhere"½X xkyk#ð46Þ,where N is the total moving steps and P is the order ofthe parametric equation. In general, P is set as 3.Therefore, the Euclidean distance from the estimatedposition of the mobile node to its approximate motion trajectory at each time is as follows:and the matrices ½A and ½b satisfy as follows:22ðX 1 X n Þ2ðY 1 Y n Þd e ðk Þ 3676 2ðX 2 X n Þ2ðY 2 Y n Þ 767,½A 6767 452ðX n 1 X n Þ 2ðY n 1 Y n Þ222X 21 X 2n Y 21 Y 2n d̂ 1,k d̂ n,k66226X 22 X 2n Y 22 Y 2n d̂ 2,k d̂ n,k½b 666 422X 2n 1 X 2n Y 2n 1 Y 2n d̂ n 1,k d̂ n,k3ð47Þ7777:775Because of the high matrix dimensions, this system isoverdetermined. So, we use the Gauss-Newton least squaremethod to arrive at the answer." #x̂ ̂ k AT AX̂yk 1ð48ÞAT b:3.3.8. Trajectory Correction Based on BP Neural Network. Byanalogy, the approximate position of the mobile node at alltimes can be obtained preliminarily.After the above coordinate calculation, the approximatepositions of mobile nodes at all times are obtained. However,in practical application, there are still a few moments whenthe positioning trajectory does not conform to the overallmotion trend. Therefore, in order to alleviate such errors,this paper adopts the BP neural network to correct the overall trajectory.We define the approximate trajectory of the mobilenode as(x ðk Þ ,ð49Þy ðk Þ,namely, the trajectory for a parameter equation aboutthe measuring interval k. For example, the coefficients ofthe parametric equation xðkÞ can be obtained by the following least square method:21 1661 266 4 1 2p ðN 1Þp 11N 1N323̂x176776 ̂x 77 coefficient 6 2 7,76 ��ffiffiffiffiffiffiffiffiffiffiffiffiffiðx k xðkÞÞ2 ðy k yðkÞÞ2 :ð51ÞN Euclidean distances are divided into two groupsthrough fuzzy C-means clustering (FCM) described above;that is, c 2 is set. Thus, the data of group A which conformsto the trajectory and group B which deviates from the trajectory can be obtained. The specific process of FCM is similarto the previous one and will not be repeated here.BP

are unknown; the other is the beacon node, whose coordi-nates are known. By transmitting signals, the distance from the mobile node can be obtained by means of time of arrival (TOA) [2, 3], time difference of arrival (TDOA) [4], angle of arrival (AOA) [5], RSSI [6, 7], and oth