Localization for mobile sensor network based on unscented Kalman filter with clipping and uncorrelated conversion

Remark 1

According to rigidity of graph, all the 10 ranges among unit N p unit are required to uniquely determine its topology. In this article, these ranges can be completed by the combination of model (2) and filter if nine of them are measured. This advantage is introduced by considering the relationship between the ranges and cosine values of angles in model (2) (see equation (46) in Appendix 1). The more the angles’ cosine values are incorporated in state, the less the ranges are required to be measurable. However, the dimension increase in state will lead to heavy computation burden and less measurements of range will lead to reduction in localization accuracy.

Because the range between each pair of sensors is greater than 2 ρ , and the cosine values of angles are between [ − 1 , 1 ] , the state vector X ~ t should satisfy the constraints

{ X ~ t ( i ) ≥ 2 ρ , i ∈ I 1 : 10 − 1 ≤ X ~ t ( i ) ≤ 1 , i ∈ I 21 : 28

(7)

For the convenience of filtering, we prefer to discretize models (2) and (6) by sampling it at instant t k , as

{ X k + 1 = F ( X k ) + G k W k Z k = H k X k + V k

(8)

with the discrete-time transition function F ( · ) , the matrices G k = δ G and H k = H , sampling period δ = t k + 1 − t k , W k = W ~ t k ~ N ( 0 , Q ) and V k = V ~ t k ~ N ( 0 , R ) . Here, the Runge–Kutta method is adopted to calculate the transition function as

F ( X k ) = X k + δ ( F k 1 + 2 F k 2 + 2 F k 3 + F k 4 ) 6

(9)

with F k 1 = f ( X k ) , F k 2 = f ( X k ) + ( δ F k 1 / 2 ) , F k 3 = f ( X k ) + ( δ F k 2 / 2 ) and F k 4 = f ( X k ) + δ F k 3 .

Remark 2

Note in physical constraint (7) that each element of state X ~ t is independent of each other. Hence, the clipping technique adopted here can be seemed as a special case of estimate projection, ²⁶ that is, equations (11) and (12) (equations (13) and (14)) are analytical solution of

χ ( · ) + = arg min χ ( · ) + { ( χ ( · ) − χ ( · ) + ) T Π ( χ ( · ) - χ ( · ) + ) }

(15)

s . t . { χ ( · ) + ( i ) ≥ 2 ρ i ∈ I 1 : 10 − 1 ≤ χ ( · ) + ( i ) ≤ 1 i ∈ I 21 : 28

(16)

with any positive definite weighting matrix Π .

After clipping, the one-step prediction of state can be calculated as

X ^ k | k − 1 = ∑ j = 0 2 n ω j m χ k | k − 1 , j +

(17)

with the weighting scalars

ω j m = { λ n + λ , j = 0 1 2 ( n + λ ) , j ∈ I 1 : 2 n

(18)

The corresponding covariance matrices can be calculated as

P k | k − 1 X = ∑ j = 0 2 n ω j c ( χ k | k − 1 , j + − X ^ k | k − 1 ) ( χ k | k − 1 , j + − X k | k − 1 ) T

(19)

And the one-step prediction of measurement is

Z ^ k | k − 1 = H k X ^ k | k − 1

(20)

Accordingly, the corresponding covariance matrices can be calculated as

P k | k − 1 Z = ∑ j = 0 2 n ω j c ( Z ^ k | k − 1 , j + − Z ^ k | k − 1 ) ( Z ^ k | k − 1 , j χ − Z ^ k | k − 1 ) T

(21)

P k | k − 1 XZ = ∑ j = 0 2 n ω j c ( χ k | k − 1 , j + − X ^ k | k − 1 ) ( Z ^ k | k − 1 , j + − Z ^ k | k − 1 ) T

(22)

with Z ^ k | k − 1 , j + = H k χ k | k − 1 , j + and

ω j c = { λ n + λ + ( 1 − α 2 + β ) , j = 0 1 2 ( n + λ ) , j ∈ I 1 : 2 n

(23)

To deeply explore the information of the measurement, the original measurement is converted uncorrelatively by introducing an effective approach and then augmented by the converted measurement. Because the converted measurement is uncorrelated with the original one, the augmentation of the original measurement can provide more useful information and then improve the accuracy of range estimation. The augmented measurement can be generated as follows.

First, the reference distribution-based UC ²² is introduced to uncorrelatively convert the original measurement. Denote Z k r as a reference random variable with the first two moments Z ¯ k r and P k Z r . To incorporate more information of Z k into Z k r , the first two moments of Z r are chosen as

Z ¯ k r = E ( Z k ) = Z ^ k | k − 1

(24)

P k Z r = cov ( Z k , Z k ) = P k | k − 1 Z

(25)

Then, the UC of the original measurement can be defined as ²²

Y k = Δ E Z k r [ g ( Z k − Z k r ) ] = ∫ p ( Z k r ) g ( Z k − Z k r ) d Z k r ≈ ∑ j = 0 2 m η j m g ( Z k − Z k , j r )

(26)

where g ( z ) = e − ( P k | k − 1 Z ) − 1 / 2 z / b is a nonlinear function with a selected positive scalar b, and p ( Z k r ) denotes the specific probability density function of Z k r

[ Z k , j r ] = [ Z ¯ k r Z ¯ k r ± γ ( m ) P k Z r ]

(27)

are the sigma points of Z k r generated by UT, and

η j m = { λ m + λ , j = 0 1 2 ( m + λ ) , j ∈ I 1 : 2 m

(28)

are the associated weighting scalars.

Remark 3

In equation (26), the converted measurement Y k is defined as the expectation of function g ( Z k − Z k r ) . When the probability density function p ( Z k r ) is unknown, the expectation can be calculated approximately by different numerical methods, for example, UT, Gaussion hypothesis quadrature rules or cubature rules. Here, the UT approximation is adopted.

According to the definition in equation (26), the mean of the UC Y k can be determined as

Y ¯ k = Δ E Z k [ Y k ] = ∫ p ( Z k ) ∫ p ( Z k r ) g ( Z k − Z k r ) d Z k r d Z k ≈ ∑ i = 0 2 m η i m ∑ j = 0 2 m η j m g ( Z k , i y − Z k , j r ) ︸ Y k , i Z

(29)

where p ( Z k ) denotes the specific probability density function of Z k

[ Z k , i y ] = [ Z ^ k | k − 1 Z ^ k | k − 1 ± γ ( m ) P k | k − 1 Z ]

(30)

are the sigma points of Z k generated by UT, η i m ’s are the associated weighting scalars as defined in equation (28), and Y k , i Z = ∑ j = 0 2 m η j m g ( Z k , i y − Z k , j r ) denotes the UC of the sigma point Z k , i y . Notice that the equivalence of the first moments between Z k and Z r (i.e. equations (24) and (25)) implies Z k , i r = Z k , i y for all i ∈ I 0 : 2 m .

Now, the augmented measurement can be constructed as the combination of the original and uncorrelatively converted measurements, that is

Z k a = [ Z k T Y k T ] T

(31)

The mean of the augmented measurement Z k a can be easily determined as Z ¯ k a = [ Z ^ k | k − 1 T Y ¯ k T ] T . The cross-covariance between X k and Z k a and the covariance of Z k a can be calculated by UT as

P k | k − 1 X Z a = [ P k | k − 1 XZ P k | k − 1 XY ]

(32)

P k | k − 1 Z a = [ P k | k − 1 Z P k | k − 1 ZY ( P k | k − 1 ZY ) T P k | k − 1 Y ]

(33)

with

P k | k − 1 XY = ∑ i = 0 2 n ω i c ( χ k | k − 1 , i + − X ^ k | k − 1 ) ( Y k | k − 1 , i + − Y ^ k | k − 1 ) T

P k | k − 1 ZY = ∑ i = 0 2 m η i c ( Z k , i y − Z ^ k | k − 1 ) ( Y k , i Z − Y ¯ k ) T

P k | k − 1 Y = ∑ i = 0 2 m η i c ( Y k , i Z − Y ¯ k ) ( Y k , i Z − Y ¯ k ) T

where Y k | k − 1 , i + = ∑ j = 0 2 m η j m g ( Z ^ k | k − 1 , i + − Z k , j r ) denotes the UC of Z ^ k | k − 1 , i + = H k χ k | k − 1 , i + , Y ^ k | k − 1 = ∑ i = 0 2 n ω i m Y k | k − 1 , i + , and

η i c = { λ m + λ + ( 1 − α 2 + β ) i = 0 1 2 ( m + λ ) i ∈ I 1 : 2 m

(34)

With the augmented measurement Z k a , the one-step prediction of state X ^ k | k − 1 can be updated by linear minimum mean square error estimator as

X ^ k = X ^ k | k − 1 + P k | k − 1 X Z a ( P k | k − 1 Z a ) − 1 ( Z k a − Z ¯ k a )

(35)

and the associated covariance matrix is calculated as

P k X = P k | k − 1 X − P k | k − 1 X Z a ( P k Z a ) − 1 ( P k | k − 1 X Z a ) T

(36)

The whole process of UKF with clipping and UC is summarized as Algorithm 1.

OPEN IN VIEWER

Algorithm 1. UKF with clipping and UC.
Input: the updated measurement range Z k at time t k ;
the estimated state vector X ^ k − 1 at time t k − 1 ;
the covariance matrix P k − 1 X at time t k − 1 .
Output: the estimated state vector X ^ k at time t k ;
the covariance matrix P k X at time t k .
1: Generate the sigma points χ k − 1 , j of state vector X ^ k − 1 as equation (10).
2: Clip the sigma points χ k − 1 , j to the bounds of constraints as equations (11) and (12).
3: Calculate propogated sigma points χ k | k − 1 , j .
4: Clip the propogated sigma points χ k | k − 1 , j as equations (13) and (14).
5: Using χ k | k − 1 , j + to calculate the one-step prediction of state vector X ^ k | k − 1 in equation (17) and measurement vector Z ^ k | k − 1 as equation (20). And the corresponding covariance matrix P k | k − 1 X in equation (19), P k | k − 1 Z in equation (21), and P k | k − 1 XZ in equation (22) can also be calculated.
6: Generate uncorrelated conversion of the original measurement Y k as equation (26).
7: Calculate the mean of the uncorrelated conversion Y ¯ k as equation (29).
8: Calculate the corresponding covariance matrix P k | k − 1 X Z a as equation (32) and P k | k − 1 Z a as equation (33).
9: Calculate the estimation of X ^ k as equation (35) and P k X as equation (36).