Object Localization and Tracking System Using Multiple Ultrasonic Sensors with Newton–Raphson Optimization and Kalman Filtering Techniques

Abstract

In this paper, an object localization and tracking system is implemented with an ultrasonic sensing technique and improved algorithms. The system is composed of one ultrasonic transmitter and five receivers, which uses the principle of ultrasonic ranging measurement to locate the target object. This system has several stages of locating and tracking the target object. First, a simple voice activity detection (VAD) algorithm is used to detect the ultrasonic echo signal of each receiving channel, and then a demodulation method with a low-pass filter is used to extract the signal envelope. The time-of-flight (TOF) estimation algorithm is then applied to the signal envelope for range measurement. Due to the variations of position, direction, material, size, and other factors of the detected object and the signal attenuation during the ultrasonic propagation process, the shape of the echo waveform is easily distorted, and TOF estimation is often inaccurate and unstable. In order to improve the accuracy and stability of TOF estimation, a new method of TOF estimation by fitting the general (GN) model and the double exponential (DE) model on the suitable envelope region using Newton–Raphson (NR) optimization with Levenberg–Marquardt (LM) modification (NRLM) is proposed. The final stage is the object localization and tracking. An extended Kalman filter (EKF) is designed, which inherently considers the interference and outlier problems of range measurement, and effectively reduces the interference to target localization under critical measurement conditions. The performance of the proposed system is evaluated by the experimental evaluation of conditions, such as stationary pen localization, stationary finger localization, and moving finger tracking. The experimental results verify the performance of the system and show that the system has a considerable degree of accuracy and stability for object localization and tracking.

1. Introduction

In many sensing technologies, object localization and tracking with range measurements have received significant attention because of their importance in many applications, such as obstacles avoidance, smart surveillance, human–computer interaction, safe sensing in factories, and autonomous navigation [1,2,3,4,5,6,7,8,9,10]. Aiming at the problem of object localization and tracking, this paper focuses on the construction of an object localization and tracking system based on an ultrasonic array. The system can locate the target by calculating the distances of ultrasonic echo signals between the sensors and the target. In this system, envelope extraction, TOF estimation, and object localization are three key issues.

Several methods for envelope extraction and TOF estimation are presented in the literature. A sensing system is designed in [11], which uses the waveform of reflected ultrasonic signals to obtain additional information about the environment, and constructs a signal classification system for objects of different materials. The Hilbert transform is used to extract the envelope of the echo signal of ultrasonic, and the traditional threshold detector is used to estimate the TOF, which can capture the echo shape reliably and accurately. However, the Hilbert transform method is not effective because it uses fast Fourier transform (FFT) and FFT inverse. The threshold method [12,13] is the simplest method to measure TOF, where TOF is determined when the echo amplitude exceeds the threshold value for the first time. Aiming at the situation where a single threshold is not robust enough to noise, a dual-threshold method [14] is proposed, which uses a parabolic approximation method to fit two points to the rising edge of the ultrasonic echo envelope. In [15], an analytic model of an ultrasonic envelope is proposed to estimate TOF with a dual-threshold method. The dual-threshold-based method [16] is also implemented in the microprocessor for dynamic distance estimation and is applied in the application of a human body. Both methods use two-level verification to correct the TOF estimation in the envelope signal. However, due to the deformation of the echo signal, the threshold-based method is often not robust. To solve the problem of threshold-based methods, the optimal correlation detection method [14,17] provides unbiased estimation using a matched filter containing a copied echo waveform that is used to determine the most likely position of the received echo signal. However, due to the variation of the echo waveform, it is necessary to store a large number of desired signal templates for correlation calculations, which consumes a large amount of computing. In [18,19,20], the cross-correlation method of transmitting and receiving signals is proposed. The sine fitting method for determining the phase shift is further mentioned in [20] as the second technique for estimating TOF with higher accuracy. To solve the problem that the frequency of noise is close to that of the desired signals and the amplitude of received signal is small and unstable, an improved energy genetic-ant colony optimization-three-cycle algorithm [21] is presented to estimate the TOF by judging the maximum energy position of the signal. However, these methods can only be applied to the standard shape of the envelope signal, but not to the envelope deformation caused by signal attenuation during the ultrasonic propagation process and the change of orientation, position, size, and material of the detected object.

Object localization is also a critical issue in this study. In general, the measurement methods in the localization problem can be classified into four categories [22], that is, time of arrival (TOA) [23], time difference of arrival (TDOA) [24], angle of arrival (AOA) [25], and received signal strengths (RSS) [26]. Several approaches are applied to the localization problem, and least-squares-based (LS) methods are widely developed in this field [27,28,29,30,31]. However, these methods are developed to apply to single-direction localization problems where the target actively sends signals to the sensors. However, the localization problem to be solved in this study is where the sensors send signals to the target and receive the echo signals to estimate the position of the target. Some of these LS methods can be modified to apply to this problem, but the performance is not satisfactory because these methods are not specifically designed for this condition.

The existing TOF estimation methods are mostly aimed at stationary targets whose ultrasonic echo signals are complete and undistorted waveforms, as shown in Figure 1. However, in practical applications, it is often impossible for the target object to be completely stationary, and the target object is often a moving object. These conditions often lead to the distortion of the envelope waveform, resulting in inaccurate TOF estimation, as shown in Figure 2. Furthermore, inaccurate TOF estimation results often lead to inaccurate object localization or unstable object tracking. This paper analyzes the most distorted ultrasonic echo waveform, and the proposed system integrates various methods to handle TOF estimation in most distortion cases, as shown in Figure 2. Furthermore, the remaining distortion cases that lead to inaccurate TOF estimation are processed by the stage of object localization. The object localization stage integrates several methods to provide a more stable and smoother result for the localization and tracking of stationary and moving objects.

In this work, an ultrasonic array system consisting of one transmitter and five receivers for object localization and tracking is implemented. The transmitter transmits ultrasonic signals, and the five receivers receive the echo signals returned by the target. The localization system uses the proposed algorithm to analyze the raw data of the echo signal. First, the system detects whether there is an echo signal through a simple VAD algorithm and then starts to locate the target by the proposed algorithms. The demodulation algorithm with a low-pass filter is used to extract the envelope of the echo signal. In order to improve the accuracy and stability of TOF estimation, the GN model and the DE model are used to characterize the ultrasonic echo signal, and the NR algorithm is applied to fit the envelope curve to estimate the parameters of the models. Furthermore, the LM modification is applied to an NR algorithm to solve the ill-conditioned problem and improve the stability of the TOF estimation. Moreover, through the analysis of various distortion waveforms, a strategy is proposed to determine the optimal fitting area according to the ideal characteristics of the GN model and the DE model, which avoids the fitting of other distorted measurement data and reduces the deviation of TOF estimation. The next stage of the proposed system is to estimate the target position; thus a discrete EKF is designed to consider the interference of TOF estimation, which uses TOF to estimate the distance to locate the target position. Aiming at the problem of outliers in the measurement, the update process of EKF is improved, which effectively detects and eliminates outliers and improves the robustness of target tracking. In summary, the following algorithms are proposed in this study: one demodulation algorithm for envelope extraction, the NRLM with two models (GN and DE) for TOF estimation, and the EKF method with outlier rejection for object localization. These algorithms are used to build a system platform, and different algorithm combinations are realized in experiments to test the accuracy and stability of object localization and tracking. These methods are compared with each other to test which combination has the best performance. Experimental results show that the proposed ultrasonic range measurement method provides stable TOF estimation under the tolerable deviation and improves the performance of target localization and tracking in the application of human finger tracking.

The proposed system and algorithms can be applied to applications such as hand-writing systems on screens or smartphones and hand gesture recognition systems. One application for the proposed system is the large-scale non-touch screen. Especially in recent days, avoiding physical contact to prevent virus infection is an important issue. This system can be built in the business district for an interactive application with the people without physical contact. It is also a safe solution for healthcare professionals to perform contactless panel control in the operating room.

The rest of the paper is organized as follows. Section 2 gives the mathematical formulation of the proposed algorithms, including the VAD, demodulation for envelope extraction, the NRLM with the GN model and the DE model for TOF estimation, and the localization method of the EKF with outlier rejection. Section 3 presents the theoretical simulation for the performance comparison of the NRLM with the GN and DE models. The experimental description and result analysis of the system are described in Section 4. Finally, the conclusion is given in Section 5.

2. Mathematical Formulation of Object Localization and Tracking Algorithms

2.1. VAD Algorithm

The drive signal is a control signal from the outside of the system to the ultrasonic transmitter; thus its start time $t_s$ is known in one signal frame. Assuming that the surrounding environment to be detected is limited by the maximum distance, the maximum distance reached by the ultrasonic signal is a fixed value of $w_d$. Assume that the ultrasonic speed is v and the sampling rate of the system is s. A signal frame f can be defined as the number of samples from the drive signal to the farthest target and back to the receiver, such that $f=2w_{d}s/v$. Therefore, when the subsequent algorithm calculates a target position, it only needs to sample f after $t_s$ as a signal frame.

Because the beam shape of the ultrasonic sensor used in this system is a spherical waveform, the ultrasonic signal reflected by the test platform and the signal directly transmitted by the transmitter to the receiver will cause a blind area. The most important factors that determine the region of the blind area are the amplitude and the number of cycles of the trigger signal of the transmitter. As shown in Figure 3, signal 1 is the interference signal which results in the blind area, and signal 2 is the reflected signal of the detected object. When the amplitude and number of cycles of the trigger signal increase, the range of signal 1 and the region of the blind area increase. If the object is in the blind area, signal 2 is indistinguishable because it overlaps with signal 1 to form a single signal. If the object is not in the blind area, the waveform of the interference signal is fixed and stable in the same system. Only by avoiding this segment of the signal, the real available reflected signal of the object can be found.

2.2. Envelope Extraction by Demodulation Algorithm

One critical point of the accuracy of the object localization is the precision of the start point time of the reflected ultrasonic signal, and the TOF can be calculated by the start point time. In general, the reflected ultrasonic signal is a set of 40 KHz signals, whose envelope must be captured first for the subsequent algorithm to calculate the start point time. To extract an envelope of the 40 KHz ultrasonic signal, the demodulation algorithm is proposed in this study. The concept of the demodulation algorithm is to extract the envelope signal by separating the high and low frequency of the original signal and retain the low-frequency part of the original signal through a low pass filter. The echo signal of the ultrasonic signal is modeled as:

$$\begin{array}{c}{r}_{dm}\left(k\right)=a\left(k\right)sin(\omega k+\varphi )\end{array}$$

(1)

where k is the time step, $a\left(k\right)$ is the amplitude; that is, envelope, $\omega$ is the center frequency of ultrasonic, and $\varphi$ is the phase shift.

Let (1) be multiplied by a sinusoidal signal and a cosine signal whose frequency is the same as that of the ultrasonic signal to generate $x_1\left(k\right)$ and $x_2\left(k\right)$, the results can be obtained as follows:

$$\begin{array}{ll}\hfill x_1\left(k\right)& =a\left(k\right)sin(\omega k+\varphi )\ast sin\left(\omega k\right)\hfill \\ \hfill & =\frac{1}{2}a\left(k\right)cos\left(\varphi \right)-\frac{1}{2}a\left(k\right)cos(2\omega k+\varphi )\hfill \end{array}$$

(2)

$$\begin{array}{ll}\hfill x_2\left(k\right)& =a\left(k\right)sin(\omega k+\varphi )\ast cos\left(\omega k\right)\hfill \\ \hfill & =\frac{1}{2}a\left(k\right)sin\left(\varphi \right)+\frac{1}{2}a\left(k\right)sin(2\omega k+\varphi )\hfill \end{array}$$

(3)

In (2) and (3), the original signal is divided into two parts, the low-frequency part with $\varphi$, and the high-frequency part with $\omega$. Therefore, a low-pass filter with a cut-off frequency around $\varphi$ can be designed to filter out the high-frequency part, and the following results can be obtained:

$$\begin{array}{c}\hfill z_1\left(k\right)=\frac{1}{2}a\left(k\right)cos\left(\varphi \right)\end{array}$$

(4)

$$\begin{array}{c}\hfill z_2\left(k\right)=\frac{1}{2}a\left(k\right)sin\left(\varphi \right)\end{array}$$

(5)

where $z_1\left(k\right)$ and $z_2\left(k\right)$ are the sample signals after passing through a low pass filter. As a consequence, the envelope $e_{dm}\left(k\right)$ can be obtained as follows:

$$\begin{array}{c}{e}_{dm}\left(k\right)=a\left(k\right)=2\sqrt{z_1{\left(k\right)}^2+z_2{\left(k\right)}^2}\end{array}$$

(6)

2.3. TOF Estimation

The algorithm of the whole system focuses on finding the start point of the reflected ultrasound signal, namely TOF estimation. In this section, the NRLM algorithm based on the GN model and the DE model is proposed and applied in the system.

2.3.1. The Envelope Model of Ultrasonic Echo Signal

In general, the GN form of the ultrasonic envelope can be modeled as an analytical expression [32], such that:

$$\begin{array}{c}{h}_g({\mathbf{z}}_{\mathbf{g}},t)=h_g(t_0,a_g,\alpha ,p,t)=a_g{\left(\frac{t-t_0}{p}\right)}^{\alpha }{e}^{\left(\frac{t-t_0}{p}\right)}\end{array}$$

(7)

where ${\mathbf{z}}_{\mathbf{g}}=[t_0,a_g,\alpha ,p]$, $t_0$ is the TOF, $a_g$ accounts for the amplitude of the ultrasonic echo signal, $\alpha$ and p are shape parameters of the specific ultrasonic transducer, and t is time. The model uses the parabolic term ${\left(\frac{t-t_0}{p}\right)}^{\alpha }$ and the exponential decay term $e^{\left(\frac{t-t_0}{p}\right)}$ to characterize the envelope waveform.

The other model used in this study is the DE model [33] such that:

$$\begin{array}{c}{h}_d({\mathbf{z}}_{\mathbf{d}},t)=h_d(t_0,a_d,\beta ,\gamma ,t)=a_d\left(e^{-\beta (t-t_0)}-e^{-\gamma (t-t_0)}\right)\end{array}$$

(8)

where ${\mathbf{z}}_{\mathbf{d}}=[t_0,a_d,\beta ,\gamma ]$, $t_0$ is the TOF, $a_d$ accounts for the amplitude of the ultrasonic echo signal, $\beta$ and $\gamma$ are the decay factors (it is noted that $\gamma$ must be larger than $\beta$), and t is time. The DE model characterizes the envelope by two exponential terms.

It is noted that the main difference between the GN model and the DE model is that they characterize the rising edge (the reign from TOF to peak) of the echo in different methods. The GN model characterizes the rising edge as a parabolic term, while the DE model characterizes the rising edge as a concave downward curve term. These two different models are applied in the proposed system to test which one is more suitable to cooperate with other algorithms in this study, and the comparison results are shown in Section 3 and Section 4.

2.3.2. NRLM Algorithm for TOF Estimation

To estimate the unknown parameters ${\mathbf{z}}_g$ in (7) and ${\mathbf{z}}_d$ in (8), the nonlinear curve-fitting algorithm using NRLM is applied on the GN model and the DE model in this study.

First, the algorithm to estimate ${\mathbf{z}}_g$ in the GN model is developed. Suppose $y_i$, $i=1\sim m$ is the measurement value of the envelope data in each frame. To formulate the curve-fitting problem with the GN model (7), the object function can be constructed as follows:

$$\begin{array}{c}\sum _{i=1}^m(y_i-h_g{\left({\mathbf{z}}_g,t_i)\right)}^2\end{array}$$

(9)

Furthermore, the nonlinear LS problem can be obtained such that:

$$\begin{array}{c}\underset{\mathbf{z}}{argmin}\sum _{i=1}^m(y_i-h_g{\left({\mathbf{z}}_g,t_i)\right)}^2\end{array}$$

(10)

which represents the minimum summation of the squared errors between measurement values and model values at the corresponding points in time. An argument vector can be constructed as: $\mathbf{r}\left({\mathbf{z}}_g\right)={[r_1\left({\mathbf{z}}_g\right)\cdots r_m\left({\mathbf{z}}_g\right)]}^{⊺}$ where $r_i\left({\mathbf{z}}_g\right)=y_i-h_g({\mathbf{z}}_g,t_i)$, and the object function (9) can be rewritten as follows:

$$\begin{array}{c}f\left({\mathbf{z}}_g\right)=\mathbf{r}{\left({\mathbf{z}}_g\right)}^{⊺}\mathbf{r}\left({\mathbf{z}}_g\right)\end{array}$$

(11)

To apply the NR algorithm for a nonlinear LS problem, the Jacobian matrix ${\mathbf{J}}_g\left({\mathbf{z}}_g\right)$ of ${\mathbf{z}}_g$, the gradient ${\nabla }_{{\mathbf{z}}_g}f\left({\mathbf{z}}_g\right)$, and Hessian matrix $\mathbf{H}\left({\mathbf{z}}_g\right)$ of $f\left({\mathbf{z}}_g\right)$ are needed to be calculated first. The Jacobian matrix ${\mathbf{J}}_g\left({\mathbf{z}}_g\right)$ is as follows:

$$\begin{array}{c}{\mathbf{J}}_g\left({\mathbf{z}}_g\right)=\left[\begin{array}{ccc}{\displaystyle \frac{\partial r_1\left({\mathbf{z}}_g\right)}{\partial z_{g1}}}& \cdots & {\displaystyle \frac{\partial r_1\left({\mathbf{z}}_g\right)}{\partial z_{gn}}}\\ ⋮& \ddots & ⋮\\ {\displaystyle \frac{\partial r_m\left({\mathbf{z}}_g\right)}{\partial z_{g1}}}& \cdots & {\displaystyle \frac{\partial r_m\left({\mathbf{z}}_g\right)}{\partial z_{gn}}}\end{array}\right]=\left[\begin{array}{cccc}{\displaystyle \frac{\partial r_1\left({\mathbf{z}}_g\right)}{\partial t_0}}& {\displaystyle \frac{\partial r_1\left({\mathbf{z}}_g\right)}{\partial a_g}}& {\displaystyle \frac{\partial r_1\left({\mathbf{z}}_g\right)}{\partial \alpha }}& {\displaystyle \frac{\partial r_1\left({\mathbf{z}}_g\right)}{\partial p}}\\ ⋮& ⋮& ⋮& ⋮\\ {\displaystyle \frac{\partial r_m\left({\mathbf{z}}_g\right)}{\partial t_0}}& {\displaystyle \frac{\partial r_m\left({\mathbf{z}}_g\right)}{\partial a_g}}& {\displaystyle \frac{\partial r_m\left({\mathbf{z}}_g\right)}{\partial \alpha }}& {\displaystyle \frac{\partial r_m\left({\mathbf{z}}_g\right)}{\partial p}}\end{array}\right]\end{array}$$

(12)

where $n=4$ in this study, and $z_{g1}=t_0$, $z_{g2}=a_g$, $z_{g3}=\alpha$, and $z_{g4}=p$, respectively. Therefore, the Jacobian matrix ${\mathbf{J}}_g\left({\mathbf{z}}_g\right)$ of this problem is a $m\times 4$ matrix with elements as follows:

$$\begin{array}{cc}\hfill {\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}_{i,1}=& \frac{a_g}{p}\left(\alpha +\frac{t_i-t_0}{p}\right){\left(\frac{t_i-t_0}{p}\right)}^{\alpha -1}{e}^{\left(\frac{t_i-t_0}{p}\right)}\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}_{i,2}=& -{\left(\frac{t_i-t_0}{p}\right)}^{\alpha }{e}^{\left(\frac{t_i-t_0}{p}\right)}\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}_{i,3}=& -a_g{\left(\frac{t_i-t_0}{p}\right)}^{\alpha }{e}^{\left(\frac{t_i-t_0}{p}\right)}ln\left(\frac{t_i-t_0}{p}\right)\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill {\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}_{i,4}=& \frac{a_g}{p}{\left(\frac{t_i-t_0}{p}\right)}^{\alpha }{e}^{\left(\frac{t_i-t_0}{p}\right)}\left(\alpha +\frac{t_i-t_0}{p}\right)\hfill \end{array}$$

(16)

where ${\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}_{i,j}$ is the $(i,j)$th element of the Jacobian matrix ${\mathbf{J}}_g\left({\mathbf{z}}_g\right)$, and $i=1\sim m$. The next step is to calculate the gradient ${\nabla }_{{\mathbf{z}}_g}f\left({\mathbf{z}}_g\right)$ as follows:

$$\begin{array}{cc}\hfill {\nabla }_{{\mathbf{z}}_g}f\left({\mathbf{z}}_g\right)& ={\left[\begin{array}{ccc}{\displaystyle \frac{\partial f\left({\mathbf{z}}_g\right)}{\partial z_{g1}}}& \cdots & {\displaystyle \frac{\partial f\left({\mathbf{z}}_g\right)}{\partial z_{gn}}}\end{array}\right]}^{⊺}={\left[\begin{array}{cccc}{\displaystyle \frac{\partial f\left({\mathbf{z}}_g\right)}{\partial t_0}}& {\displaystyle \frac{\partial f\left({\mathbf{z}}_g\right)}{\partial a_g}}& {\displaystyle \frac{\partial f\left({\mathbf{z}}_g\right)}{\partial \alpha }}& {\displaystyle \frac{\partial f\left({\mathbf{z}}_g\right)}{\partial p}}\end{array}\right]}^{⊺}\hfill \\ \hfill & ={\left[\begin{array}{cccc}2\sum _{i=1}^m{r}_i\left({\mathbf{z}}_g\right){\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial t_0}}& 2\sum _{i=1}^m{r}_i\left({\mathbf{z}}_g\right){\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial a_g}}& 2\sum _{i=1}^m{r}_i\left({\mathbf{z}}_g\right){\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial \alpha }}& 2\sum _{i=1}^m{r}_i\left({\mathbf{z}}_g\right){\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial p}}\end{array}\right]}^{⊺}\hfill \\ \hfill & =2{\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}^{⊺}\mathbf{r}\left({\mathbf{z}}_g\right)\hfill \end{array}$$

(17)

Furthermore, the Hessian matrix $\mathbf{H}\left({\mathbf{z}}_g\right)$ is defined as:

$$\begin{array}{c}\mathbf{H}\left({\mathbf{z}}_g\right)=\left[\begin{array}{ccc}{\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial z_{g1}\partial z_{g1}}}& \cdots & {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial z_{g1}\partial z_{gn}}}\\ ⋮& \ddots & ⋮\\ {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial z_{gn}\partial z_{g1}}}& \cdots & {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial z_{gn}\partial z_{gn}}}\end{array}\right]=\left[\begin{array}{cccc}{\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial t_0\partial t_0}}& {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial t_0\partial a_g}}& {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial t_0\partial \alpha }}& {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial t_0\partial p}}\\ ⋮& ⋮& ⋮& ⋮\\ {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial p\partial t_0}}& {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial p\partial a_g}}& {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial p\partial \alpha }}& {\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial p\partial p}}\end{array}\right]\end{array}$$

(18)

and the $(k,j)$th element of the Hessian matrix $\mathbf{H}\left({\mathbf{z}}_g\right)$ can be expressed by using indices k and j such that:

$$\begin{array}{cc}\hfill \mathbf{H}{\left({\mathbf{z}}_g\right)}_{k,j}& ={\displaystyle \frac{{\partial }^{2}f\left({\mathbf{z}}_g\right)}{\partial z_{gk}\partial z_{gj}}}={\displaystyle \frac{\partial }{\partial z_{gk}}}\left({\displaystyle \frac{f\left({\mathbf{z}}_g\right)}{\partial z_{gj}}}\right)=2\sum _{i=1}^m\left({\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial z_{gk}}}{\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial z_{gj}}}+r_i\left({\mathbf{z}}_g\right){\displaystyle \frac{{\partial }^2{r}_i\left({\mathbf{z}}_g\right)}{\partial z_{gk}\partial z_{gj}}}\right)\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill & =2\sum _{i=1}^m\left({\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial z_{gk}}}{\displaystyle \frac{\partial r_i\left({\mathbf{z}}_g\right)}{\partial z_{gj}}}\right)+2\mathbf{G}{\left({\mathbf{z}}_g\right)}_{k,j}\hfill \end{array}$$

(20)

where $\mathbf{G}{\left({\mathbf{z}}_g\right)}_{k,j}={\sum }_{i=1}^m{r}_i\left({\mathbf{z}}_g\right){\displaystyle \frac{{\partial }^2{r}_i\left({\mathbf{z}}_g\right)}{\partial z_{gk}\partial z_{gj}}}$ is defined as the $(k,j)$th element of matrix $\mathbf{G}\left({\mathbf{z}}_g\right)$. Therefore, the Hessian matrix $\mathbf{H}\left({\mathbf{z}}_g\right)$ can be obtained as the following form:

$$\begin{array}{c}\mathbf{H}\left({\mathbf{z}}_g\right)=2{\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}^{⊺}{\mathbf{J}}_g\left({\mathbf{z}}_g\right)+\mathbf{G}\left({\mathbf{z}}_g\right)\end{array}$$

(21)

As a consequence, the recursive formula for the NR algorithm applied to the nonlinear LS problem is as follows:

$$\begin{array}{c}{\mathbf{z}}_g^{(k+1)}={\mathbf{z}}_g^{(k)}-{({\mathbf{J}}_g{({\mathbf{z}}_g^{(k)})}^{⊺}{\mathbf{J}}_g({\mathbf{z}}_g^{(k)})+\mathbf{G}({\mathbf{z}}_g^{(k)}))}^{-1}{\mathbf{J}}_g({\mathbf{z}}_g^{(k)})\mathbf{r}({\mathbf{z}}_g^{(k)})\end{array}$$

(22)

where the notation $\left(k\right)$ and $(k+1)$ denote the kth and $(k+1)$th time steps. In general, the term $\mathbf{G}\left({\mathbf{z}}_g\right)$ in (21) is significantly small because it contains the second derivative terms and can be ignored in practical applications. However, ignoring the term $\mathbf{G}\left({\mathbf{z}}_g\right)$ in (21) may cause ${\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}^{⊺}{\mathbf{J}}_g\left({\mathbf{z}}_g\right)$ to become a singular matrix and make the system crash. This problem can be solved by using the LM modification, which adds a sufficiently large number $\lambda$ into the diagonal of ${\mathbf{J}}_g{\left({\mathbf{z}}_g\right)}^{⊺}{\mathbf{J}}_g\left({\mathbf{z}}_g\right)$ such that:

$$\begin{array}{c}{\mathbf{z}}_g^{(k+1)}={\mathbf{z}}_g^{(k)}-{({\mathbf{J}}_g{({\mathbf{z}}_g^{\left(k\right)})}^{⊺}{\mathbf{J}}_g({\mathbf{z}}_g^{\left(k\right)})+{\lambda }_k\mathbf{I})}^{-1}{\mathbf{J}}_g({\mathbf{z}}_g^{\left(k\right)})\mathbf{r}({\mathbf{z}}_g^{\left(k\right)})\end{array}$$

(23)

This term ${\lambda }_k\mathbf{I}$ can make sure that the search always points to the descent direction, and it can be viewed as an approximation to $\mathbf{G}\left({\mathbf{z}}_g\right)$ in (22). Therefore, (23) is the ongoing, recursive formula for the TOF estimation $t_0$.

The above procedure also can be applied on the DE model, and (23) is modified as:

$$\begin{array}{c}{\mathbf{z}}_d^{(k+1)}={\mathbf{z}}_d^{\left(k\right)}-{({\mathbf{J}}_d{({\mathbf{z}}_d^{\left(k\right)})}^{⊺}{\mathbf{J}}_d({\mathbf{z}}_d^{\left(k\right)})+{\lambda }_k\mathbf{I})}^{-1}{\mathbf{J}}_d({\mathbf{z}}_d^{\left(k\right)})\mathbf{r}({\mathbf{z}}_d^{\left(k\right)})\end{array}$$

(24)

where

$$\begin{array}{cc}\hfill {\mathbf{J}}_d{\left({\mathbf{z}}_d\right)}_{i,1}=& a_d(\gamma e^{-\gamma (t_i-t_0)}-\beta e^{-\beta (t_i-t_0)})\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\mathbf{J}}_d{\left({\mathbf{z}}_d\right)}_{i,2}=& e^{-\gamma (t_i-t_0)}-e^{-\beta (t_i-t_0)}\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill {\mathbf{J}}_d{\left({\mathbf{z}}_d\right)}_{i,3}=& a_d{e}^{-\beta (t_i-t_0)}(t_i-t_0)\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill {\mathbf{J}}_d{\left({\mathbf{z}}_d\right)}_{i,4}=& -a_d{e}^{-\gamma (t_i-t_0)}(t_i-t_0)\hfill \end{array}$$

(28)

to estimate the TOF $t_0$. (23) and (24) are tested with the aforementioned algorithms in this study, and the performances of the simulation and experimental results are compared in Section 3 and Section 4, respectively.

2.3.3. Determination of Suitable Fitting Window

In the proposed system, when the amplitude of the ultrasonic echo signal is sufficiently large to be detected, the target is judged to exist by the system, and the TOF is estimated by the proposed algorithm. However, the waveform of the ultrasonic echo signal is always distorted because of different reflective surfaces, which results in the multiple-peaks phenomenon and interference results of TOF estimation. Therefore, instead of fitting the entire envelope data, a suitable fitting window located on the expected peak according to the mathematical properties of the envelope model is better. For example, reconsidering the GN model (7), which has only one peak value, the curve from the peak to the beginning is monotonically decreasing; that is, there is no turning point. Therefore, as shown in Figure 4, the ideal shape of the waveform in the fitting window should be a single peak and monotonically decrease from the peak; otherwise, a large fitting error will be caused, and the accuracy of TOF estimation will be affected, as shown in Figure 5.

To solve the problem of an incorrect fitting window with multi-peaks, a three-step procedure is proposed in this study. In Figure 4, Figure 5 and Figure 6, the blue line is the echo envelope, the red line is the result of curve-fitting, ${\widehat{t}}_0$ is the estimated TOF by (23) or (24), $t_0$ is the ground truth of TOF, which is calculated from target coordinate measured manually, $(t_{max},V_{max})$ is the peak value of echo envelope, and $(t^{*},V^{*})$ is the instant that characterizes the onset of echo envelope, which is calculated by the two-maximums algorithm [15]. The size of the fitting window is defined from $t^{*}$ to $t_{max}+n$, where n is a suitable number. The three-step procedure is presented as follows:

(1)

Set the maximum amplitude detected for the first time as the initial value $(t_{max0},V_{max0})$, and find the corresponding $(t^{*},V^{*})$ by the two-maximums algorithm.

(2)

In the ith iteration, check if the waveform from $t_{maxi}$ to $t^{*}$ is a monotonic decrease. If it is a monotonic decrease, then go to step 3.

(3)

Execute (23) or (24) to obtain the TOF ${\widehat{t}}_0$.

Through the above three-step process, the suitable fitting window can be found, and the best fitting result can be obtained, as shown in Figure 6.

2.4. Object Localization and Tracking Algorithm

When the TOFs of all sensor channels are estimated by the NRLM methods with the GN model or the DE model, the position of the target can be estimated by these TOFs. In this section, the NRLM algorithm can also be used in target position estimation. In addition, the EKF algorithm is also proposed for object localization and tracking. The performance of these two proposed methods is compared with the other methods in Section 4.

2.4.1. NRLM Algorithm for Object Localization

Suppose v is the wave propagation speed of ultrasonic, $\mathbf{x}$ is the target object position, which is unknown, ${\mathbf{x}}_t$ is the position of the ultrasonic transmitter, ${\mathbf{x}}_{ri}$ is the position of the ultrasonic receiver where $i=1\sim m$, and m is the number of receivers, and $\mathbf{x}$, ${\mathbf{x}}_t$ and ${\mathbf{x}}_{ri}\in {\mathbb{R}}^{n\times 1}$ (in general applications, $n=2$ or 3), and ${\tau }_i$ is the traveling time, which can be measured from the transmitter to the object and back to the receiver i. The round trip equation can be formulated as follows:

$$\begin{array}{c}{h}_{ti}\left(\mathbf{x}\right)=\frac{1}{v}\left(\right|\mathbf{x}-{\mathbf{x}}_{ri}|+|\mathbf{x}-{\mathbf{x}}_t\left|\right)\end{array}$$

(29)

which presents the traveling time from the transmitter to the object and back to the receiver i. Therefore, the problem can be constructed as the minimum summation of the squared errors between measured traveling times with the corresponding receivers such that:

$$\begin{array}{c}\underset{\mathbf{x}}{argmin}\sum _{i=1}^m({\tau }_i-h_{ti}{\left(\mathbf{x})\right)}^2\end{array}$$

(30)

From Section 2.3.2, a similar procedure can be applied to solve this problem by a recursive formula to estimate the object position $\mathbf{x}$.

2.4.2. EKF Algorithm for Object Localization and Tracking

In this section, the EKF algorithm is used to estimate the position and velocity of the target. Suppose the state $\mathsf{\theta }={\left[\begin{array}{cccc}{x}_1& x_2& {\dot{x}}_1& {\dot{x}}_2\end{array}\right]}^{⊺}$ where $\mathbf{x}={\left[\begin{array}{cc}{x}_1& x_2\end{array}\right]}^{⊺}$ is the position coordinate and ${\left[\begin{array}{cc}{\dot{x}}_1& {\dot{x}}_2\end{array}\right]}^{⊺}$ is the velocity of target. The state transition equation can be written as follows:

$$\begin{array}{c}{\mathsf{\theta }}^{\left(k\right)}=\mathbf{A}{\mathsf{\theta }}^{(k-1)}+{\mathbf{w}}^{(k-1)}\end{array}$$

(31)

where

$$\begin{array}{c}{\mathsf{\theta }}^{\left(k\right)}=\left[\begin{array}{c}{x}_1^{\left(k\right)}\\ x_2^{\left(k\right)}\\ {\dot{x}}_1^{\left(k\right)}\\ {\dot{x}}_2^{\left(k\right)}\end{array}\right],\mathbf{A}=\left[\begin{array}{cccc}1& 0& t& 0\\ 0& 1& 0& t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],{\mathsf{\theta }}^{(k-1)}=\left[\begin{array}{c}{x}_1^{(k-1)}\\ x_2^{(k-1)}\\ {\dot{x}}_1^{(k-1)}\\ {\dot{x}}_2^{(k-1)}\end{array}\right]\end{array}$$

(32)

where t is the transition time, the notations $\left(k\right)$ and $(k-1)$ denote the kth and $(k-1)$th time steps, and $\mathbf{w}\in {\mathbb{R}}^{4\times 1}$ is the state noise, which is Gaussian distribution with zero mean and diagonal covariance matrix $\mathbf{Q}\in {\mathbb{R}}^{4\times 4}$. The measurement equation is:

$$\begin{array}{c}{\mathbf{u}}^{\left(k\right)}={\mathbf{h}}^{(k-1)}+{\mathbf{v}}^{(k-1)}\end{array}$$

(33)

where $\mathbf{h}={\left[\begin{array}{c}{h}_{t1}\left(\mathbf{x}\right)\cdots h_{tm}\left(\mathbf{x}\right)\end{array}\right]}^{⊺}$, and $\mathbf{v}\in {\mathbb{R}}^{m\times 1}$ is the observation noise, which is Gaussian distribution with zero mean and diagonal covariance matrix $\mathbf{R}\in {\mathbb{R}}^{m\times m}$.

When the TOFs are obtained from each channel, outliers may occur when the echo distortion is under serious conditions, such as complex shaped targets or target movement, affecting the performance of the EKF. A reliable and efficient method is provided in [34] for outlier detection. The prediction step (31) and (33) in the proposed EKF are modified as follows:

$$\begin{array}{cc}\hfill {\tilde{\mathsf{\theta }}}^{\left(k\right)}& =\mathbf{A}{\mathsf{\theta }}^{(k-1)}\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\tilde{\mathbf{u}}}^{\left(k\right)}& ={\tilde{\mathbf{h}}}^{(k-1)}\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\tilde{\mathbf{P}}}^{\left(k\right)}& ={\mathbf{Q}}^{\left(k\right)}\hfill \end{array}$$

(36)

where $\tilde{\mathsf{\theta }}={\left[\begin{array}{cccc}{\tilde{x}}_1& {\tilde{x}}_2& {\dot{\tilde{x}}}_1& {\dot{\tilde{x}}}_2\end{array}\right]}^{⊺}$, $\tilde{\mathbf{h}}={\left[\begin{array}{c}{h}_{t1}\left(\tilde{\mathbf{x}}\right)\cdots h_{tm}\left(\tilde{\mathbf{x}}\right)\end{array}\right]}^{⊺}$, $\tilde{\mathbf{x}}={\left[\begin{array}{cc}{\tilde{x}}_1& {\tilde{x}}_2\end{array}\right]}^{⊺}$, and ${\mathbf{Q}}^{\left(k\right)}$ is the noise variance of ${\mathsf{\theta }}^{\left(k\right)}$. The Kalman update process is as follows:

$$\begin{array}{cc}\hfill {\tilde{\mathbf{S}}}^{\left(k\right)}& ={\left({\mathbf{C}}^{\left(k\right)}{\tilde{\mathbf{P}}}^{\left(k\right)}{\left({\mathbf{C}}^{\left(k\right)}\right)}^{⊺}+\frac{1}{w_k}{\mathbf{R}}^{\left(k\right)}\right)}^{-1}\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\tilde{\mathbf{K}}}^{\left(k\right)}& ={\tilde{\mathbf{P}}}^{\left(k\right)}{({\mathbf{C}}^{\left(k\right)})}^{⊺}{\tilde{\mathbf{S}}}^{\left(k\right)}\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill {\mathsf{\theta }}^{\left(k\right)}& ={\tilde{\mathsf{\theta }}}^{\left(k\right)}+{\tilde{\mathbf{K}}}^{\left(k\right)}({\mathbf{u}}^{\left(k\right)}-{\tilde{\mathbf{u}}}^{\left(k\right)})\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill {\mathbf{P}}^{\left(k\right)}& =(\mathbf{I}-{\tilde{\mathbf{K}}}^{\left(k\right)}{\mathbf{C}}^{\left(k\right)}){\tilde{\mathbf{P}}}^{\left(k\right)}\hfill \end{array}$$

(40)

where

$$\begin{array}{c}\hfill {\mathbf{C}}^{\left(k\right)}=\left(\sum _{i=1}^k{w}_i{\tilde{\mathbf{u}}}_i{\tilde{\mathsf{\theta }}}_i^{⊺}\right){\left(\sum _{i=1}^k{w}_i{\tilde{\mathsf{\theta }}}_i{\tilde{\mathsf{\theta }}}_i^{⊺}\right)}^{-1}\end{array}$$

(41)

and ${\tilde{\mathbf{u}}}_i$ is the ith element of $\tilde{\mathbf{u}}$, ${\tilde{\mathsf{\theta }}}_i$ is the ith element of $\tilde{\mathsf{\theta }}$, and $w_k$ is a weighting such that:

$$\begin{array}{c}\hfill w_k={\displaystyle \frac{a+0.5}{b+{({\mathbf{u}}^{\left(k\right)}-{\tilde{\mathbf{u}}}^{\left(k\right)})}^{⊺}{\mathbf{R}}^{\left(k\right)}({\mathbf{u}}^{\left(k\right)}-{\tilde{\mathbf{u}}}^{\left(k\right)})}}\end{array}$$

(42)

and the mth element of diagonal of ${\mathbf{R}}^{\left(k\right)}$ is $r_{km}$ such that:

$$\begin{array}{c}\hfill {\mathbf{r}}_{km}={\displaystyle \frac{1}{k}}\sum _{i=1}^k{w}_i{\left({\tilde{\mathbf{u}}}_{im}-{\mathbf{C}}^{\left(k\right)}(m,:){\tilde{\mathsf{\theta }}}_i\right)}^2\end{array}$$

(43)

where ${\mathbf{C}}^{\left(k\right)}(m,:)$ is the mth row of the matrix ${\mathbf{C}}^{\left(k\right)}$. Here, ${\tilde{\mathbf{K}}}^{\left(k\right)}$ is the Kalman gain, ${\mathbf{S}}^{\left(k\right)}$ is the posterior covariance of the residual prediction error, and $a=b=1$ is suggested in [34].

In the Kalman update process, (42) presents that if the prediction error in ${\mathbf{u}}^{\left(k\right)}$ increases, the weighting $w_k$ of that data sample will decrease. If the prediction error is too large and dominates the denominator, the weighting $w_k$ will become very small. Furthermore, the posterior covariance of the residual prediction error ${\tilde{\mathbf{S}}}^{\left(k\right)}$ will become very small to result in a very small Kalman gain ${\tilde{\mathbf{K}}}^{\left(k\right)}$ and decrease the influence of the data sample ${\mathbf{u}}^{\left(k\right)}$ when predicting ${\mathsf{\theta }}^{\left(k\right)}$.

3. Theoretical Comparison of TOF Estimation with Simulation

To compare the performances of TOF estimation by the proposed NRLM algorithm with the GN model and the DE model, the envelope data extracted from a complete and undistorted echo signal of ultrasonic, such as Figure 1, is used to test the simulation with different signal-to-noise-ratios (SNR). In the simulation, the performances are tested with the root mean square error (RMSE) between the estimated TOF and the real TOF. The RMSE is defined as follows:

$$\begin{array}{c}\mathrm{RMSE}=\sqrt{\frac{1}{N}\sum _{i=1}^N\Vert {{\widehat{t}}_0-t}^2\Vert }\end{array}$$

(44)

where ${\widehat{t}}_0$ is the estimated TOF, t is the real TOF, and $N=1000$ is the number of simulation runs at the specified noise level. In the simulation, the noises are added to the envelope signal with different SNRs, which are defined as follows:

$$\begin{array}{c}\mathrm{SNR}=20\log (\frac{e_{max}}{n_{std}})\end{array}$$

(45)

where $e_{max}$ is the maximum amplitude of the envelope and $n_{std}$ is the standard deviation of the noise. The simulation results are presented in Figure 7. It is observed that the proposed NRLM algorithm with the DE model has better performance than that with the GN model when SNR is low, and they have similar performances when SNR is high. Therefore, it can be concluded that the proposed NRLM algorithm with the DE model has superior flexibility than that with the GN model.

4. Experiment Results

The architecture of the experiment system using ultrasonic sensors and the real experiment platform are shown in Figure 8. As shown in the left figure of Figure 8, there are one transmitter (red) and five receivers (blue) mounted on the edge of a 60 × 38 ${\mathrm{cm}}^2$ platform with a U-shaped sensor rack. The ultrasonic sensors used in this system are Pro-wave 400PT160, which is a transceiver with a center frequency of 40 KHz such that they can operate as the transmitter or the receiver [35]. The NI data acquisition (DAQ USB-6366) is connected between the computer and all sensors. The DAQ is used to drive the transmitter by a computer with a rectangular burst consisting of eight cycles, acquire the raw data of analog echo signal from sensors, and convert these analog data into digital data and send them to the computer. In the following subsections, there are three cases to be tested such that:

Case 1:

The localization target is a stationary marker pen that stands vertically on the platform at measured coordinates. This testing object is stationary and has a smooth surface.

Case 2:

The localization target is one human index finger stationary pointing vertically to the platform at measured coordinates. This testing object is stationary and has an uneven surface.

Case 3:

The system will track one moving human finger with different trajectories, and the index finger is pointing vertically to the platform. This testing object is moving and has an uneven surface.

There are 700 testing frames in Case 1 and Case 2 and 300 testing frames in Case 3.

4.1. The Performance of TOF Estimation

In this section, the fixed threshold method [12,13], the two-maximums algorithm [15], the energy genetic-ant colony optimization-three-cycle (EGACO) algorithm with the GN model [21], the proposed NRLM algorithm with the GN model (23), and the proposed NRLM algorithm with the DE model (24) are tested to compare the standard deviations of the TOF estimation in Case 1 and Case 2, where the targets are all stationary. All approaches tested here are applied to the same fitting window. The standard deviations of the testing results of five receiver channels are shown in Figure 9 and Figure 10. It is noted that the vertical axis presents the standard deviations of distances with the corresponding TOFs in Figure 9 and Figure 10 for intuitive observation.

It is observed that the threshold method has a larger variation in TOF estimation when it is the target in both cases. Since the threshold method uses a fixed threshold value to detect TOF, it can not handle most situations. Regardless of whether the target is a pen or a human finger, the amplitude of the interference wave, which is greater than the threshold value, seriously affects the estimation results and produces some outliers. The results of TOF estimation of the two-maximums algorithm are relatively stable and more accurate than those of the threshold method because it applies linear interpolation based on the envelope model on the rising edge to estimate the TOF, and the rising edge is also located in the ideal echo region from the method in Section 2.3.3. However, the linear interpolation used in the two-maximums algorithm is very sensitive to changes in the slope of the rising edge, resulting in corresponding interference to TOF estimation.

In Figure 9 and Figure 10, it is observed that the proposed NRLM algorithm with the DE model has better performance than the other methods in most cases, including the EGACO algorithm with the GN model and the proposed NRLM algorithm with the GN model. The EGACO uses the GN model and estimates the TOF by judging the maximum energy position of the signal. The result of EGACO is similar to that of the threshold method in many cases because it estimates the TOF by signal energy, which is easily affected by the shape of the signal, especially when the object is in the non-stationary state. The curve-fitting results of the NRLM algorithm with the GN model and the DE model are shown in Figure 11. The GN model characterizes the actual envelope better than the DE model, such that the residual error of curve-fitting using (7) is smaller than that of (8). However, the GN model is more sensitive to the variation of the measured echo signal because it fits the onset of the echo well and interferes with the TOF. On the other hand, although the TOF estimation of the DE model has a certain deviation, the rising edge of the model is less sensitive to the variation of the echo, which is consistent with the simulation result in Section 3. Therefore, it can be concluded that the NRLM algorithm with the DE model provides a more stable TOF estimation than other methods, and the TOF data estimated by the NRLM algorithm with the DE model is used in all cases in the object localization and tracking in Section 4.2.

4.2. The Performance of Object Localization and Tracking

In this section, the concept of the LS method [27], the proposed NRLM method, and the proposed EKF method are used to estimate the target position. The TOF results of all the five receiver channels are used among the three methods, and the issue of outlier rejection is then discussed.

Table 1. Case 1—RMSE of target coordinates (X, Y) for the marker pen (unit: cm).

Target Localization Method		LS	NRLM	EKF
Target Coordinate	Axis
(14, 37)	x	0.56	0.52	0.51
	y	0.44	1.51	0.45
(18, 37)	x	0.26	0.20	0.21
	y	1.16	1.58	1.12
(14, 41)	x	0.09	0.07	0.10
	y	0.72	0.56	0.28
(18, 41)	x	0.24	0.23	0.22
	y	0.05	0.60	0.08

and

Table 2. Case 2—RMSE of target coordinates (X, Y) for human index finger (unit: cm).

Target Localization Method		LS	NRLM	EKF
Target Coordinate	Axis
(14, 37)	x	0.28	0.36	0.28
	y	1.31	0.97	0.89
(18, 37)	x	0.30	0.30	0.27
	y	0.35	0.94	0.45
(14, 41)	x	0.58	0.59	0.44
	y	1.11	0.60	0.23
(18, 41)	x	0.13	0.17	0.14
	y	0.28	0.39	0.03

present the RMSE of the target position with three methods in Cases 1 and 2 to show the accuracy of the object localization methods.

Table 3. Case 1—Standard deviations of target coordinates (X, Y) for the marker pen (unit: cm).

Target Localization Method		LS	NRLM	EKF
Target Coordinate	Axis
(14, 37)	x	0.415	0.325	0.087
	y	2.028	0.264	0.085
(18, 37)	x	0.584	0.500	0.187
	y	3.130	0.334	0.154
(14, 41)	x	0.313	0.259	0.103
	y	1.282	0.158	0.063
(18, 41)	x	0.567	0.903	0.206
	y	1.405	0.301	0.049

and

Table 4. Case 2—Standard deviations of target coordinates (X, Y) for human index finger (unit: cm).

Target Localization Method		LS	NRLM	EKF
Target Coordinate	Axis
(14, 37)	x	1.191	1.336	0.400
	y	3.514	0.498	0.143
(18, 37)	x	0.995	0.968	0.216
	y	3.521	0.430	0.222
(14, 41)	x	1.734	1.036	0.574
	y	5.203	0.448	0.292
(18, 41)	x	1.449	1.178	0.718
	y	4.146	0.530	0.246

present the standard deviations of the target position with the three methods in Cases 1 and 2 to show the stability of the object localization methods. It is observed that most RMSEs and all standard deviations of the EKF method are much smaller than those of the other methods in both cases. Therefore, the proposed EKF method has better performance in both accuracy and stability than the other methods for object localization.

Figure 12, Figure 13 and Figure 14 present the results of target tracking with different moving directions for Case 3, which are denoted as Case 3-1, Case 3-2, and Case 3-3. It should be noted that in all test trajectories of Case 3, the finger stops for a period of time at the start and end points before and after the movement. In the LS method, there is a system matrix containing distance estimation, which increases the uncertainty of the estimation and easily causes ill-conditioned problems. Because the round-trip model does not include any distance estimation, the NRLM method is superior to the LS method. However, the NRLM method only estimates coordinates based on the current TOF data, and the estimation results are directly affected by the measurement noise. Although the performance of the NRLM method and the EKF method in the stationary state of the target is similar, the EKF estimates the position of the target based on the previous state of the target, and inherently considers the interference of the measurement, which can provide more stable and smoother localization and tracking of the moving target. It is noticed that in Figure 14, the tracking in the x-direction is delayed more than the tracking in the y-direction. This is because the geometry of the ultrasound platform structure makes the variance in the x-direction larger than that in the y-direction [36], which can also be observed in Table 2 and Table 4.

Here, the outlier issue is reconsidered in Case 3-3. When the target moves from (20, 36) to (20, 48), it is observed that sometimes the echo signal is severely distorted and results in generating several outliers in the measurement data. The outliers have a great influence on the tracking performance of the EKF method. In Figure 15, the outlier rejection method proposed in Section 2.4.2 is used to improve the tracking performance of the target. As shown in the left figure of Figure 15, it can be observed that when the outliers are detected and eliminated, the variance of the x-coordinate is relatively small, and the estimated target trajectory can follow the desired path more smoothly and more consistently than that in the right figure of Figure 14. It is noted that since there are no obvious outliers in the measurement by the EKF method in Case 3-1 and Case 3-2, the results of the tracking will be similar before and after outlier rejection.

Another test trajectory is shown in the right figure of Figure 15. The estimated target coordinates are around the reference trajectory. However, there is a delay in the x-direction near the end point. This is caused by the constant occurrence of outliers as the target moves. Because the Kalman gain is smaller in the presence of outliers, the estimated coordinates are closer to the predicted trajectory.

In Figure 15, it can be observed that there are differences between the real trajectories and the estimated trajectories, but the estimated tracking trajectories are quite stable, which means that the proposed algorithm can be fairly stable to localize the detected object. In this work, the key issue for object localization is not only to directly estimate the accurate position but also to estimate the stable position. In reality, position estimation is prone to deviation due to the influences of various factors, such as the size of the sensors and the geometry of sensor positions. It is not practical to consider these factors because it makes the system complicated and not easy to solve. However, in a fixed system, these influences are also fixed and always result in a fixed offset in position estimation with a fixed position. Thus, even if there is an error between the position estimation and the real position, the error can be directly compensated by an offset calibration to achieve the correct object localization and tracking. In Figure 15 and Table 3 and Table 4, it can be observed that the standard deviations of the EKF method are very small. The smaller the standard deviation, the better the stability of the position estimation. Therefore, although there is a difference between the real trajectories and the estimated trajectories, the difference can be easily compensated by offset calibration because of the good stability of position estimation.

5. Conclusions

An ultrasonic array system consisting of one transmitter and five receivers for object localization and tracking is proposed in this paper. The system uses the VAD algorithm to detect the target’s echo signal and locate the target. The envelope is then extracted by the demodulation algorithm, and the TOF estimation is performed by the NRLM algorithm with the GN model or the DE model. Furthermore, the detected object can be located by the NRLM algorithm or the EKF algorithm with outlier rejection. In this study, these proposed methods are compared and experimentally evaluated in different scenarios for TOF estimation and tracking on target position. The NRLM algorithm that uses nonlinear curve-fitting provides a more reasonable TOF than other methods that use linear interpolation. Although the GN model is more suitable for the rising edge than the DE model, the TOF estimation of the GN model is more sensitive to the variation of the rising edge. The EKF method uses previous states to decrease the influence of the current interference of measurement. Moreover, outlier rejection provides a more stable and smoother result for the localization and tracking of moving targets. Therefore, the experiment results verify that the NRLM algorithm with the DE model has the best performance for TOF estimation, and the EKF algorithm outperforms the other methods in object localization.