Signal, Image and Video Processing https://doi.org/10.1007/s11760-018-1272-2

ORIGINAL PAPER

An ultrasonic positioning algorithm based on maximum correntropy criterion extended Kalman filter weighted centroid Fuqiang Ma1 · Fangjie Liu1 · Xiaotong Zhang1

· Peng Wang1 · Hongying Bai1 · Hang Guo1

Received: 31 July 2017 / Revised: 1 March 2018 / Accepted: 7 March 2018 © Springer-Verlag London Ltd., part of Springer Nature 2018

Abstract Ultrasonic positioning technology is being used in a wide range of application areas. In an ultrasonic positioning system, the noise of an ultrasound wave may not follow a Gaussian distribution but has a strong impulse because of many factors. A traditional extended Kalman filter based on the minimum mean square error would produce a linear estimation error and cannot handle a non-Gaussian noise effectively. Therefore, we propose a novel maximum correntropy criterion extended Kalman filter weighted centroid positioning algorithm based on a new Kalman gain formula to determine the maximum correntropy criterion. The maximum correntropy criterion maps the signal to a high-dimensional space and effectively deals with the non-Gaussian noise in ultrasonic positioning. In addition, the weighted centroid uses the results of the extended Kalman filter as inputs and reduces the impact of the linear estimation error on the positioning results. Experimental results show that the maximum correntropy criterion extended Kalman filter weighted centroid algorithm can improve the positioning accuracy by 60.06% over the extended Kalman filter and 22.83% compared with the maximum correntropy criterion extended Kalman filter. Overall, the proposed algorithm is more robust and effective. Keywords Non-Gaussian noise · Maximum correntropy criterion (MCC) · Extended Kalman filter (EKF) · Weighted centroid · Ultrasound

1 Introduction Due to its flexibility, scalability and other features, wireless positioning technology is used for many applications, including warehouse goods management, fireman positioning, patient monitoring. Currently, global positioning system (GPS), which offers high positioning accuracy, good timeliness, and strong anti-interference, is widely used in many fields [1,2]. However, GPS is not always used in the node positioning of wireless sensor network (WSN) due to many limits of sensor nodes. In addition, it is difficult to use GPS in some specific environments such as indoor positioning. Indoor positioning technologies include ultrasonic positioning technology, infrared positioning technology, Bluetooth positioning technology, and UWB positioning technology. Ultrasonic ranging technology has been widely used as an efficient, accurate, and non-contact method in various application domains because of its advantages like simple

B 1

Xiaotong Zhang [email protected]

structure, easy realization, and low cost. Therefore, this paper focuses on indoor positioning based on ultrasonic positioning technology. We presume that the position of fixed ultrasonic anchor node in WSN is already known, and then locate the mobile robot. Utilizing ultrasonic ranging technology, we can determine the distance between anchor nodes and the mobile robot. Finally, based on the ranging results and the motion state of the mobile robot, we can determine the position of the mobile robot [3]. There are many conventional ultrasonic filter algorithms including Kalman filter algorithm (KF) [4], extended Kalman filter algorithm (EKF) [5] and particle filter algorithm (PF) [6]. The conventional ultrasonic filter algorithms can effectively predict position if the observation noise and process noise follow Gaussian distribution. However, if the noise is non-Gaussian, the performance of conventional ultrasonic filtering algorithms will be severely degraded. For example, KF algorithm can only deal with the noise in linear systems [7] and cannot handle a non-Gaussian noise effectively. In a 3D ultrasonic positioning system, the ultrasonic noise often does not obey a Gaussian distribution because of multi-path effect, disturbance of air and temperature, cir-

University of Science and Technology Beijing, Beijing, China

123

Signal, Image and Video Processing

cuitry interferences and other factors. Thus, it is difficult for a traditional positioning algorithm to effectively solve the positioning problem of non-Gaussian noise in a nonlinear system [8]. Many researchers have tried to improve the conventional ultrasonic filter algorithms. The KF algorithm based on the maximum correntropy criterion (MCCKF) that can handle the high-order statistics of noise to allow effective treatment of non-Gaussian noise have been proposed in [9,10]. They use correntropy instead of the minimum mean square error (MMSE) as the optimal criterion in the filter to account for higher-order information from the noise. Liu et al. [11] proposed the maximum correntropy extended Kalman filter (MCEKF) to handle nonlinear functions. Menegaz et al. proposed an unscented Kalman filter (UKF) [12] that can filter non-Gaussian noise while keeping the mean and variance characteristics. On the other hand, UKF algorithm needs a collection of sampling points which would increase computational complexity and is difficult to be applied to a real-time positioning system. And the maximum correntropy unscented Kalman filter algorithm (MCUKF) has been proposed [13,14]. Kim and Kim proposed a dynamic hybrid ultrasonic extended Kalman filter algorithm [15]. This algorithm can improve the positioning accuracy of a mobile robot by dynamic distance estimation. Obviously, MCC is an important method to improve conventional filter algorithms with the effect of outliers. But MCC in filter algorithms is not enough for high accuracy. In order to improve positioning accuracy in an effective way, this paper proposes a novel maximum correntropy criterion extended Kalman filter weighted centroid positioning algorithm (MCWC). In this paper, we design a maximum correntropy criterion extended Kalman filter (MCCEKF) which uses the maximum correntropy criterion (MCC) to replace the traditional MMSE as the cost function for EKF [16]. MCCEKF adopts Gaussian kernel function as the cost function to handle a non-Gaussian noise which is different from MCEKF [11] and then utilizes weighted centroid algorithm to improve positioning accuracy. MCC can effectively deal with higher-order statistics of signal, so MCWC offers good robustness for non-Gaussian noise. EKF will produce a linear estimation error due to the use of the first-order Taylor expansion, which ignores second-order and higher-order statistics, then the filtering error will increase. The weighted centroid algorithm is used to calculate the centroid position by using the distance of the multiple positioning results as the weight [17,18]. Based on the design and the structural characteristics of the ultrasonic receiving device, our strategy applies the weighted centroid algorithm to EKF. The experimental data show that the weighted centroid algorithm can effectively reduce estimation error and improve the positioning accuracy.

123

Fig. 1 An ultrasonic ranging system

This paper is organized as follows. In Sect. 2, we briefly introduce the ultrasonic positioning system of the mobile robot. In Sect. 3, we consider the basic principles of EKF based on MMSE. In Sect. 4, we describe MCC. In Sect. 5, we propose a novel maximum correntropy criterion extended Kalman filter weighted centroid positioning algorithm (MCWC). In Sect. 6, we provide experimental results and analyze the performances of different positioning algorithms. In Sect. 7, we summarize the MCWC positioning algorithm.

2 System model As shown in Fig. 1, a mobile robot consists of a speed sensor, an electronic compass and some ultrasonic receivers. A robot’s motion state is determined by speed and direction which are measured by the speed sensor and electronic compass. There are eight ultrasonic receivers on the top of the robot, spaced in a two-dimensional disk at a 45◦ angle, and the radius of the disk is RU . Each ultrasonic probe is capable of receiving ultrasonic signal independently. An ultrasonic positioning system consists of a mobile robot and several ultrasonic transmitters (T xi ) that are fixed on the top of the room and side of the wall and their three-dimensional coordinates are already known. Eight ultrasonic probes of the mobile robot can receive the ultrasonic signal from the transmitters in real time. As shown in Fig. 2, the fixed reference coordinate system is OG and the relative reference system for a mobile robot is OR . x-axis direction of the relative coordinate is parallel to the movement direction of the mobile robot, and y-axis direction is vertical to the movement direction, and z-axis of the mobile robot is 0. The position of a mobile robot can be expressed as M = [x, y, θ ]T . x and y are parameters of the fixed reference coordinate system. θ is the clockwise angle between two x-axes of OG and OR . Ultrasonic ranging system uses TDOA (time difference of arrival) technology, and di j is the distance between the ith ultrasonic transmitter and the jth ultrasonic probe.

Signal, Image and Video Processing

function.  represents the driving noise matrix. Yk+1 represents the observed value at time k + 1. The process noise and observation noise are Gaussian random processes with mean zero, variance Q and R, respectively. In other words, Wk and Vk+1 are independent Gaussian white noise sequences. E[Wk ] = E[Vk+1 ] = 0

(4)

E[Wk WkT ] = T ] E[Vk+1 Vk+1

Q

(5)

=R

(6)

First, EKF linearizes the nonlinear model using local linear features. And the state equation makes a first-order Taylor expansion with the nonlinear function f (k, X k ) around the filter value Xˆ k : X k+1 ≈ f (k, Xˆ k ) + Fig. 2 Structure of ultrasonic positioning

The coordinate of the ultrasonic probe U can be expressed as (1): MU j = [x + RU ∗ cos[θ − ( j − 1) ∗ 45], y + RU ∗ sin[θ − ( j − 1) ∗ 45]]T

∂f (X k − Xˆ k ) + (k, Xˆ k )Wk ∂ Xˆ k

∂f ∂ f (k, Xˆ k ) = | Xˆ k =X k = k+1|k ∂ Xˆ k ∂ Xˆ k ∂f ∂ f (k, Xˆ k ) = | X k = Xˆ k Xˆ k = φk f (k, Xˆk ) − ∂ Xk ∂ Xˆ k

(7) (8) (9)

The equation of state is: (1) X k+1 = k+1|k X k + k Wk + φk

where j is the jth receiving ultrasonic probe. MU j is the ith position of the ultrasonic probe. x and y represent the coordinates of the center point of ultrasonic disk.

3 Extended Kalman filter

Similarly, the process equation makes a first-order Taylor expansion with the nonlinear function h(k +1, X k+1 ) around the filter value Xˆ k+1 : Yk+1 = h(k + 1, Xˆ k+1 ) +

In order to deal with the filtering problem of nonlinear system, we often use EKF to transform a nonlinear system into an approximate linear system. The goal of EKF is to obtain a more accurate position estimation by using dynamic information of objectives. First, we expand the state transition function and process function into a Taylor series and ignore the second-order and higher-order statistics. Then, EKF tries to remove the effect of noise based on MMSE [19]. The dynamic equations of the discrete nonlinear system can be expressed as: X k+1 = f (k, X k ) + k Wk

(2)

Yk+1 = h(k + 1, X k+1 ) + Vk+1

(3)

X k+1 is the state value at time k + 1. f (k, X k ) represents the nonlinear state transition function. X k is the state value at time k. h(k + 1, X k+1 ) represents the nonlinear prediction

(10)

∂h = Hk+1 ∂ Xˆ k+1 yk+1 = h(k + 1, Xˆ k+1 ) −

∂h (X k+1 − Xˆ k+1 ) + Vk+1 ∂ Xˆ k+1 (11) (12) ∂h ˆ X k+1 ∂ Xˆ k+1

(13)

The process equation is: Yk+1 = Hk+1 Xˆ k+1 + yk+1 + Vk+1

(14)

In linear system equations, the Jacobian matrix of f (k, X k ) and h(k + 1, X k+1 ) can substitute for the state transition matrix k+1|k and observation matrix Hk+1 . We suppose there are n-dimensional state variables, namely X = [x1 , x2 , . . . , xn ]T , so that the corresponding Jacobian matrices can be expressed as follows:

123

Signal, Image and Video Processing

⎡ ∂ f1

∂ f1 ⎤ ∂ xn ∂ f2 ⎥ ∂ xn ⎥ ⎥

.. ⎥ . ⎦

(15)

∂ fn ∂ xn ⎤ ∂h 1 ∂ xn ∂h 2 ⎥ ∂ xn ⎥ ⎥

.. ⎥ . ⎦

(16)

(23)

According to the linear model of the system, we can obtain the recursive equation of EKF (17–22): The step-state transition equation is:

E[.] is the expectation operator, κ(., .) is a translation function of shift-invariant Mercer kernel, p X ,Y (., .) is the joint probability density function of x and y. In this paper, Gaussian kernel is used as the kernel function:

x − y2 (24) κ(x, y) = G σ (x − y) = exp − 2σ 2

k+1|k =

∂f =⎢ . ∂X ⎣ ..

∂ fn ∂ x1

⎡ ∂h ∂x

Hk+1

∂ f1 ∂ x2 ∂ f2 ∂ x2

∂x ⎢ ∂ f12 ⎢ ∂ x1 ⎢

1

⎢ ∂h12 ⎢ ∂ x1 ∂H =⎢ = ⎢ .. ∂X ⎣ .

.. .

∂ fn ∂ x2 ∂h 1 ∂ x2 ∂h 2 ∂ x2

∂h n ∂ x1

.. .

∂h n ∂ x2

··· ··· .. . ··· ··· ··· .. . ···

∂h n ∂ xn

Xˆ k+1|k = f ( Xˆ k|k )

(17)

The state update equation is: Xˆ k+1|k+1 = Xˆ k+1|k + K k+1 εk+1

(18)

where εk+1 = Yk+1 − h( Xˆ k+1|k )

(19)

(20)

(21)

(22)

K k+1 is a gain matrix. εk+1 is the residual value between the estimated value and the observed value. k+1|k is a state transition matrix at time k + 1. Hk+1 is a prediction matrix at time k + 1. Pk+1|k is the covariance matrix of the prediction error estimate from time k to time k +1. Pk|k is the covariance matrix of the prediction error estimate at time k. The advantage of EKF is that the nominal trajectory does not need to be calculated in advance, but it can only be used when the filtering error and the step prediction error are small.

4 The maximum correntropy criterion (MCC) Information theoretic learning (ITL) is a nonparametric system framework based on correntropy and divergence.

123

(25)

When using the Gaussian kernel, the probability density function of kernel estimation is: (26)

i=1

N is the sample of x and y. We can get the equa{xi , yi }i=1 tion by Taylor series expansion:

The covariance matrix update equation is: Pk+1|k+1 = [In − K k+1 Hk+1 ]Pk+1|k

N 1  κσ (xi , yi ) Vˆ N ,σ (X , Y ) = N

N 1  G σ (xi − yi ) Vˆ N ,σ (X , Y ) = N

The step prediction covariance matrix is: T Pk+1|k = k+1|k Pk|k k+1|k + Q k+1

Euclidean norm is defined as ., and σ is the bandwidth of the Gaussian kernel. In most applications, the joint probability density function px,y (., .) is unknown and is limited by the amount of sample data [22]. The probability density function of kernel estimation is typically used:

i=1

The filter gain matrix is: T T K k+1 = Pk+1|k Hk+1 [Hk+1 Pk+1|k Hk+1 + Rk+1 ]−1

Correntropy provides a kind of generalized correlation function for ITL. The maximum correntropy criterion is a novel nonlinear local optimal measurement between two random variables, X and Y [20,21]. It can be defined as follows:  κ(X , Y ) p X ,Y (x, y)dxdy V (X , Y ) = E X Y [κ(X , Y )] =

1 Vˆσ (X , Y ) = √ 2π σ

∞  (−1)n E[(X − Y )2n ] 2n σ 2n n!

(27)

n=0

Correntropy criterion is the sum of weights of random variables X − Y at all time points. The kernel bandwidth σ is the weighting factor of second-order and higher-order statistics. When the kernel bandwidth σ is sufficiently large, the second-order statistics are dominant. The essence of correntropy criterion is that data are mapped to high-dimensional linear space by the kernel function. EKF can maximize the value of correntropy function between desired signals and filter the result by applying correntropy criterion as a cost function. This optimal criterion is called MCC.

Signal, Image and Video Processing −1 −1 T Rk+1 [h( Xˆ k+1|k ) Xˆ k+1|k+1 + Mk+1 Hk+1 Pk+1|k

5 MCWC algorithm As a result of non-Gaussianity, the deviation of observation noise is large. If we use EKF directly to estimate the target position, the error would be very large and the curve of moving target will not be flat. In this situation, the accuracy and stability of a location system will significantly decrease. MCWC uses MCC as a criterion and adopts Gaussian kernel function as the cost function to handle a non-Gaussian noise. This strategy utilizes MCCEKF to handle the data of eight receiving ultrasonic probes and uses the results as inputs for the weighted centroid algorithm. Finally, we can obtain the center position of the mobile robot. EKF based on MMSE can effectively deal with Gaussian noise model. However, when the noise is non-Gaussian, the performance of EKF will degrade significantly. The MMSE also cannot effectively handle large outliers. In order to overcome these drawbacks, we use MCC to improve robustness and effectiveness of EKF. MCWC uses the propagation equation of the state mean and the covariance matrix to obtain the prior estimation state of the filter, and then uses MCC to obtain filter gain. Due to the local similar measurement of MCC, MCWC can effectively remove impulsive noise and large outliers. According to the Gaussian kernel function and the weight matrix of EKF −1 −1 , Pk+1|k , the cost function of MCWC can be obtained: Rk+1

(28)

To minimize the objective function, we take the derivative of target variable Xˆ k+1|k : 1 −1 T G σ (Yk+1 − h( Xˆ k+1|k+1 ) R −1 )Hk+1 Rk+1 (Yk+1 k+1 σ2 1 − h( Xˆ k+1|k+1 )) − 2 G σ ( Xˆ k+1|k+1 σ −1 − f ( Xˆ k|k ) P −1 )Pk+1|k ( Xˆ k+1|k+1 − f ( Xˆ k|k )) = 0 k+1|k

(29) −1 −1 Pk+1|k f ( Xˆ k|k ) Xˆ k+1|k+1 − Pk+1|k −1 T = Mk+1 Hk+1 Rk+1 (Yk+1 − h( Xˆ k+1|k+1 ))

(30)

We can get Mk+1 :

Mk+1 =

G σ (Yk+1 − h( Xˆ k+1|k+1 ) R −1 ) k+1

G σ ( Xˆ k+1|k+1 − f ( Xˆ k|k ) P −1 )

Since the state transition equation is a linear equation, we can get (33) through (32): −1 −1 T Pk+1|k Rk+1 [h( Xˆ k+1|k ) Xˆ k+1|k+1 + Mk+1 Hk+1

+ Hk+1 ( Xˆ k+1|k+1 − Xˆ k+1|k )] −1 −1 T Rk+1 Yk+1 Xˆ k+1|k + Mk+1 Hk+1 = Pk+1|k

(33)

The optimal estimation Xˆ k+1|k+1 at time k + 1 is (33): −1 Xˆ k+1|k+1 = Xˆ k+1|k + (Pk+1|k −1 −1 T T + Mk+1 Hk+1 Rk+1 Hk+1 )−1 Mk+1 Hk+1 Rk+1

(Yk+1 − h( Xˆ k+1|k ))

(34)

According to (34), the state update equation and covariance matrix of MCCKEF can be expressed as: Xˆ k+1|k+1 = Xˆ k+1|k + K k+1 (Yk+1 − h( Xˆ k+1|k ))

(35)

−1 −1 T R −1 H −1 T K k+1 = (Pk+1|k + Mk+1 Hk+1 k+1 k+1 ) Mk+1 Hk+1 Rk+1

Pk+1|k+1 = (In − K k+1 Hk+1 )Pk+1|k (In − K k+1 Hk+1 )T

k+1

k+1|k

(32)

(36)

Jm = G σ (Yk+1 − h( Xˆ k+1|k+1 ) R −1 ) + G σ ( Xˆ k+1|k+1 − f ( Xˆ k|k ) P −1 )

+ Hk+1 ( Xˆ k+1|k+1 − Xˆ k+1|k )] −1 −1 T f ( Xˆ k|k ) + Mk+1 Hk+1 Rk+1 Yk+1 = Pk+1|k

(31)

k+1|k

According to Eq. (14), we can obtain the optimal estimation of EKF by a first-order Taylor expansion with nonlinear functions h( Xˆ k+1|k+1 ) around the filter value Xˆ k+1|k :

T + K k+1 Rk+1 K k+1

(37)

Equations (17, 21, 35–37) comprise the basic formulas of MCWC, and Eqs. (15–16) are auxiliary formulas. Out of the nonlinearity, information of 3D ultrasonic model is inaccurate, incomplete, and system parameters will vary when EKF transforms the nonlinear model into an approximate linear model. EKF will produce a linear estimation error due to the use of linear model of first-order Taylor expansion that ignores second-order and higher-order statistics. Therefore, EKF has a greater estimation error which results in deviation of the filtering trajectory from the actual trajectory of the mobile robot and produces sub-optimal filtering results. Based on the design and the structural characteristics of the ultrasonic receiving devices, this paper proposes an EKF algorithm based on weighted centroid. The weighted centroid algorithm can effectively reduce estimation error and improve positioning accuracy. In a 3D ultrasonic positioning system, we use Eq. (17) to calculate the state estimated position O  of the center of the disk at time k + 1 by using the center position of the ultrasonic receiving disk at the time k. Then, the state estimated position of eight receiving ultrasonic probes at time k + 1 can be calculated by Eq. (1). Each ultrasonic probe is capable of receiving the ultrasonic

123

Signal, Image and Video Processing

Fig. 3 Ultrasonic probe filtering position with MCWC

signal from T xi , and we can get the corresponding observation values. Finally, we can obtain the optimal estimation Xˆ k+1,i of eight receiving ultrasonic probes at time k + 1 by MCWC. As is shown in Fig. 3, RU is the ultrasonic disk radius, and Ri is the distance from the estimated center of the disk O  to the ith ultrasonic probe filter position. Each distance |RU − Ri | can be used as the weighting factor of the weighted centroid algorithm. The weighting factor of each ultrasonic probe can be expressed as (38): 1 |RU −Ri | 1 i=1 |RU −Ri |

wi = 8

(38)

By using the weighted centroid algorithm in MCWC, we can obtain the center position of the mobile robot. Xˆ k+1,U =

8 

wi Xˆ k+1,i

(39)

i=1

Xˆ k+1,U is the optimal estimation of the center position at time k + 1, and Xˆ k+1,U can be used as a state transition element for next moment. MCC is different from the traditional kernel method which relies on Welsch M estimation, and MCC focuses on the high density part of samples to reduce the effect of outliers. Meanwhile, EKF can solve the problem of nonlinear model. The weighted centroid algorithm can effectively reduce the estimation error of linearization in EKF and improve positioning accuracy. Figure 4 illustrates the flowchart of MCWC algorithm.

123

Fig. 4 Flowchart of MCWC algorithm

6 Experiment Based on the theory above, performances of MCWC algorithm are next tested by simulation and experiment. NonGaussian noise obeys a SαS distribution with a dispersion that is equivalent to variance in the simulation. Suppose the mobile robot undergoes a uniform linear motion in a twodimensional plane. The horizontal and vertical velocities of the mobile robot are 2 m/s, which start from the coordinate origin. Using these settings, 500 Monte Carlo simulations are performed to calculate the average result for each experiment. The performance of MCCEKF is compared with EKF without the weighted centroid algorithm. The kernel bandwidth of MCCEKF is set to 1. Figure 5 illustrates the root mean square error (RMSE) of EKF and MCCEKF. The mean square error (MSE) of EKF is much greater than that of MCCEKF. The MCCEKF exhibits 48.24% better positioning accuracy than EKF. Figure 6 illustrates the error probability density of EKF and MCCEKF. RMSE of MCCEKF exhibits a more concentrated distribution, but EKF is relatively discrete. In order to make MCC more suitable for MCWC, a simulation experiment is performed that varies the size of kernel bandwidth of Gaussian kernel function. Figure 7 illustrates that MCWC is more accurate when the kernel bandwidth is

Signal, Image and Video Processing 1.2

1

Gaussian kernel 0.2 Gaussian kernel1 Gaussian kernel2 Gaussian kernel3

1

0.8

Root mean square error

Root mean square error/m

EKF MCCEKF

0.6

0.4

0.2

0.8

0.6

0.4

0.2

0

0 0

10

20

30

40

50

60

70

0

80

10

20

30

40

50

60

80

Fig. 8 Errors for different Gaussian kernel bandwidths

Fig. 5 MSEs of EKF and MCCEKF 2

1

Root mean square error

pdf of EKF pdf of MCCEKF

1.5

PDF

70

Time

Time

1

0.5

0 -1

-0.5

0

0.5

1

1.5

Error

EKF UKF MCCEKF MCWC

0.8

0.6

0.4

0.2

0 0

10

20

30

40

50

60

70

80

Time

Fig. 6 Error probability density functions

Fig. 9 RMSE values of four algorithms

0.48

Root mean square error

0.47 0.46 0.45 0.44 0.43 0.42 0.41 0.4 0.39

0

0.5

1

1.5

2

2.5

Gaussian kernel

Fig. 7 Average error of Gaussian kernel bandwidth

3

3.5

4

set to 1. The data shown in Fig. 8 illustrate the RMSE of MCWC when the Gaussian kernel bandwidth is set to 0.2, 1, 2, or 3. For a kernel bandwidth of 1, RMSE of the MCWC is significantly smaller than that for other bandwidth values. Thus, the Gaussian kernel bandwidth is set to 1 for subsequent experiments. We also perform experiments in an indoor environment. The motion direction of the mobile robot is measured by the electronic compass. The velocity difference between the present time and the last time can be determined by the speed sensor. Therefore, we can obtain the step-state transition in the horizontal and the vertical direction. The data shown in Fig. 9 illustrate that the MCWC algorithm can effectively reduce the estimation error of the first-order Taylor expansion, handle large outliers of noise, and improve the overall positioning accuracy. The data shown in Fig. 10 illustrate that the MCWC algorithm has a con-

123

Signal, Image and Video Processing 0.1

3.5 3

Error of horizontal direction/m

pdf of EKF pdf of UKF pdf of MCCEKF pdf of MCWC

PDF

2.5 2 1.5 1 0.5 0 -1

-0.5

0

0.5

1

0

-0.1

-0.2

-0.3

-0.4

-0.5

1.5

0

10

20

30

Error

40

50

60

70

80

50

60

70

80

Time

Fig. 10 Probability densities of four algorithms

Fig. 12 Error in the horizontal direction 0.14

80 True MCWC

0.12

Error of vertical direction/m

70 60 50 40 30

0.1 0.08 0.06 0.04 0.02

20

0

10

-0.02

0

10

20

30

40

Time

0 0

10

20

30

40

50

60

70

80

Fig. 13 Error in the vertical direction Fig. 11 Curve of MCWC algorithm

centrated distribution of error and a smaller error with an average RMSE of only 0.1617. The positioning accuracy of the MCWC algorithm improves by 60.06% over EKF, 22.83% compared with MCCEKF, and 53.71% compared to UKF. As shown in Fig. 11, the mobile robot performs uniform linear motion. In each experiment, a sampling frequency of the ultrasound of 1 time per second and an 80 measurements are used. The black curve is the real trajectory, and the red curve is trajectory of MCWC algorithm. It can be seen that the MCWC algorithm can effectively eliminate the outliers of observation noise and make the filtering trajectory closer to the real trajectory. As shown in Figs. 12, 13, the error in the horizontal direction increases slightly with time, but remains in acceptable range compared with the simulation results. However, there are more errors within a small range in the vertical direc-

123

tion. Experimental results show that the MCWC algorithm can improve the accuracy and stability of the ultrasonic positioning system. The MCWC algorithm can effectively handle a non-Gaussian noise model. In this paper, RMSE is minimum when the Gaussian kernel bandwidth of MCWC is set to 1. The MCWC algorithm which combines MCCEKF and weighted centroid algorithm effectively reduces the linear estimation error.

7 Conclusion This paper describes a novel MCWC positioning algorithm, designed to solve non-Gaussian noise in an ultrasonic positioning system. MCWC algorithm maps the signal to a high-dimensional space and processes the second-order and higher-order statistics of noise by using MCC to improve the state estimation accuracy. Meanwhile, the weighted cen-

Signal, Image and Video Processing

troid algorithm reduces the linear estimation error of EKF. Experimental results show that the Gaussian kernel bandwidth has a great impact on non-Gaussian noise. RMSE can be minimized when the Gaussian kernel bandwidth is set to 1. MCWC algorithm is robust and effective and can suppress the impact of non-Gaussian noise in a ultrasonic positioning system. MCWC provides 60.06% improved positioning accuracy compared to EKF and 22.83% improved positioning accuracy compared with MCCEKF. Acknowledgements This research is supported by National Key R&D Program of China (2016YFB0700500). The authors would like to thank the reviewers for their comments.

References 1. Xu, C., He, J., Zhang, X.: Geometrical kinematic modeling on human motion using method of multi-sensor fusion. Information Fusion. 41(5), 243–254 (2017a) 2. Xu, C., He, J., Zhang, X.: Toward near-ground localization: modeling and applications for TOA ranging error. IEEE Trans. Antennas Propagation. 65(10), 5658–5662 (2017b) 3. Kumar, A.A., Krishna, M.S.: Target tracking in a WSN with directional sensors using electronic beam steering. In: 2012 Fourth International Conference on Communication Systems and Networks (COMSNETS). IEEE (2012) 4. Mahfouz, S., et al.: Target tracking using machine learning and Kalman filter in wireless sensor networks. IEEE Sens. J. 14(10), 3715–3725 (2014) 5. Reif, K., Unbehauen, R.: The extended Kalman filter as an exponential observer for nonlinear systems. IEEE Trans. Signal Process. 47(8), 2324–2328 (1999) 6. Yang, P.: Efficient particle filter algorithm for ultrasonic sensorbased 2D range-only simultaneous localisation and mapping application. IET Wirel. Sensor Syst. 2(4), 394–401 (2012) 7. Fakoorian, S.A. et al.: Ground reaction force estimation in prosthetic legs with an extended Kalman filter. In: 2016 Annual Systems Conference (SysCon). IEEE (2016)

8. Jimnez, A.R., Seco, F.: Ultrasonic positioning methods for accurate positioning. Instituto de Automatica Industrial, Madrid (2005) 9. Reza, I. et al.: Kalman filtering based on the maximum correntropy criterion in the presence of non-Gaussian noise. In: 2016 Annual Conference on Information Science and Systems (CISS). IEEE pp. 500–505 (2016) 10. Chen, B., Liu, X., Zhao, H., Principe, J.C.: Maximum correntropy Kalman filter. Automatica 76, 70–77 (2017) 11. X. Liu, H. Qu, J. Zhao, B. Chen.: Extended Kalman filter under maximum correntropy criterion. In: Proceedings of the 2016 International Joint Conference on Neural Networks, Vancouver, Canada. pp. 1733–1737 (2016) 12. Menegaz, H.M.T.: A systematization of the unscented Kalman filter theory. IEEE Trans. Autom. Control 60(10), 2583–2598 (2015) 13. Liu, X., Chen, B., Xu, B., Wu, Z., Honeine, P.: Maximum correntropy unscented filter. Int. J. Syst. Sci. 48(8), 1607–1615 (2017) 14. Wang, G., Li, N., Zhang, Y.: Maximum correntropy unscented Kalman and information filters for non-Gaussian measurement noise. J. Franklin Inst. 354(18), 8659–8677 (2017) 15. Kim, S.J., Kim, B.K.: Dynamic ultrasonic hybrid localization system for indoor mobile robots. IEEE Trans. Ind. Electron. 60(10), 4562–4573 (2013) 16. Liu, W., Pokharel, P.P., Prncipe, J.C.: Correntropy: properties and applications in non-Gaussian signal processing. IEEE Trans. Signal Process. 55(11), 5286–5298 (2007) 17. Wang, J., et al.: Performance analysis of weighted centroid algorithm for primary user localization in cognitive radio networks. In: 2010 Conference Record of the Forty Fourth Asilomar Conference on Signals, Systems and Computers (ASILOMAR). IEEE, pp. 966–970 (2010) 18. Wang, J., et al.: Weighted centroid localization algorithm: theoretical analysis and distributed implementation. IEEE Trans. Wirel. Commun. 10(10), 3403–3413 (2011) 19. Julier, S.J., Jeffrey, K.U.: A new extension of the Kalman filter to nonlinear systems. Int. Symp. Aerosp. Def. Sens. Simul. Controls. 3(26), 182–193 (1997) 20. Cinar, G.T., Prncipe, J.C.: Hidden state estimation using the Correntropy Filter with fixed point update and adaptive kernel size. In: The 2012 International Joint Conference on Neural Networks (IJCNN), IEEE pp. 1–6 (2012) 21. Wang, P., et al.: Adaptive time delay estimation algorithm for indoor near-field electromagnetic ranging. Int. J. Commun. Syst. 30(5), (2017) 22. Rao, M., et al.: A test of independence based on a generalized correlation function. Signal Process. 91(1), 15–27 (2011)

123