利用多個超音波感測器與卡曼濾波器實現單一物體之追蹤

國立交通大學

電控工程研究所

碩士論文

利用多個超音波感測器與卡曼濾波器

實現單一物體之追蹤

Single Object Tracking Using Multiple Ultrasonic

Sensors and Kalman Filtering Techniques

研 究 生：林 冠 宏

指導教授：胡 竹 生 博士

利用多個超音波感測器與卡曼濾波器

實現單一物體之追蹤

Single Object Tracking Using Multiple Ultrasonic

Sensors and Kalman Filtering Techniques

研究生：林 冠 宏 Student： Kuan-Hung Lin

指導教授：胡竹生 博士 Advisor： Prof. Jwu-Sheng Hu

國 立 交 通 大 學

電 控 工 程 研 究 所

碩 士 論 文

A Thesis

Submitted to Institute of Electrical and Control Engineering

College of Electrical Engineering and Computer Science

National Chiao Tung University

in partial Fulfillment of the Requirements

for the Degree of Master

in

Electrical and Control Engineering

July 2013

Hsinchu, Taiwan, Republic of China

i

利用多個超音波感測器與卡曼濾波器

實現單一物體之追蹤

研究生：林 冠 宏 指導教授：胡竹生 博士

國立交通大學

電控工程研究所碩士班

摘 要

本論文之目的為利用超音波測距原理，在長 59 公分，寬 32.4 公分之大型面板 上，定位與追蹤物體所在的二維座標。這個系統包含了裝置在螢幕邊框的一個超音 波發射器與五個接收器，藉由每個通道之時差測距，計算出物體的座標位置。超音 波在傳送過程中的衰減，以及不同目標物形狀、大小與相對於感測器之方向，皆會 對超音波回聲產生干擾並造成波形之變化，進而影響到時差測距的準確度。本論文 針對以上之問題提出一個新的時差測距之估測方法，可尋找出合理的反射波區間， 再藉由反射波包絡之雙指數模型與牛頓最佳化演算法，得到更精準與穩定的時差測 距。本論文亦使用擴展型卡曼濾波器來估測物體座標位置，可同時考慮時差測距之 干擾與離群值的問題對於座標位置之影響，有效地在雜訊環境中提升物體定位與追 蹤之穩定度。最後藉由不同情境的實驗來測試本系統之表現，包含了以麥克筆與人 類手指作為靜止目標物之位置估測，與移動的人類手指之追蹤。

ii

Single Object Tracking Using Multiple Ultrasonic Sensors

and Kalman Filtering Techniques

Student： Kuan-Hung Lin

Advisor： Prof. Jwu-Sheng Hu

Institute of Electrical and Control Engineering

National Chiao-Tung University

Abstract

This thesis realizes 2D coordinate localization and tracking of single object on a 59-centimeters-long and 32.4-centimeters-wide large panel based on the principle of ultrasonic range measurement. There are 1 ultrasonic transmitter and 5 receivers equipped on the edge of the panel. The time-of-flight (TOF) from each channel is first derived and the target coordinate is then obtained. TOF estimation is inaccurate due to shape distortion of echoes waveform, which is caused by attenuation during propagation and also varies with target type, size, location, and orientation. A new method of TOF is presented to solve the above problem. This method provides a more accurate and steady TOF estimation by fitting the double exponential model on the reasonable region of envelope using Newton-Ralphson optimization. An Extended Kalman Filter is also designed to estimate the target coordinate inherently accounting the interference and outlier issue of the derived TOF, and effectively reduces the disturbance to target localization in critical measurement condition. The system performance was assessed through experimental evaluation of several scenarios, including localization of stationary marker pen, stationary human’s finger, and tracking on moving human’s finger.

iii

誌 謝

本論文能夠順利的完成，首先要感謝的是指導教授胡竹生老師。在這兩年的碩 士生涯裡，老師給了我在研究上的許多訓練與栽培，讓我學習到一個研究生該如何 獨立思考、有邏輯有效率的發現與解決問題。而實驗室所提供豐富的資源，也讓我 能在研究的過程中更加平穩順利。感謝口試委員溫瓌岸老師與蕭得聖老師的寶貴建 議與指教，使得本論文能夠更臻完美。 兩年的碩士生涯中，XLAB 就像是我第二個家了，在這期間所受到的幫忙與照 顧就如同家人一般的溫馨。感謝一進實驗室就帶著我做計劃的阿法學長，對於研究 方法的認真與嚴謹，一步步讓我的研究打下了良好的基礎；而阿法所製作過的蛋糕， 其媲美米其林的滋味更是讓人難以忘懷。感謝阿吉學長在我研究路程上的熱心幫忙， 每每與學長討論的過程總能讓我激發出新的想法，對於問題的思考與解決能更有 sense。因為大學部專題而跟 XLAB 第一次接觸，感謝 Judo 學長能夠容忍我們這三 個小屁孩，在做了快三年的專題路上不斷的給予指導，最難忘記的，就是一起奮鬥 的夜晚的海宴與麥當勞；縝密堅固的思考邏輯常常讓小弟跟學長報告的時候都戰戰 兢兢，深怕一不小心口誤被打槍。但也因為如此，也讓我在表達能力或研究上的思 考都能夠再更加進步。感謝明唐學長，在對於研究方法的處理與問題的分析給與許 多指引，還有關於人生與感情方面上的分享，著實讓小弟受益良多。感謝大師兄晉 源學長，雖然領域不同，在研究的路程上也給與許多關照。感謝昭男學長與耕維學 長，很有想法與抱負的你們讓我每每跟你們聊天都有許多收穫，機智又風趣的 style 也常帶來許多歡樂。感謝哲鳴學長、宗翰學長與建廷學長，不會忘記一起打球、一 起談心、一起唱歌、一起辦聯誼、一起遊玩與聊天打屁的日子。還要謝謝跟我們同 年卻提早一屆畢業的 Daniel，除了能聊天練英文之外，你的那句至理名言：「因為 不知道老師的標準在哪，因此不要為自己設限，就盡力去做吧~」更成為我研究的 動力。

iv 同一屆畢業的夥伴們，這兩年一起成長、一同打拼，讓我研究之路不孤獨。 從 大 學 一 起 轉 系 過 來 的 小 師 兄 期 元 與 室 友 哲 宇 ， 一 起 加 入 同 一 個 專 題 ， 為 了 conference 與龍騰比賽一起努力，最後還進入同一個實驗室，一路上互相扶持照應， 種種回憶歷歷在目;在大學時代就一起在胡老師門下做專題的鳴遠，不但風趣幽默， 有時對事物都能一針見血， 為苦悶研究生活增添不少樂趣；遠自大馬來的阿文， 是 XLAB 的健身教練~專業級的健身指導讓大夥可以用正確有效的方式鍛鍊身體； 3C 達人大夢，總有跟你有聊不完的科技玩意兒話題，很多共同興趣與想法也常一 起分享；調酒師小山東，不會忘記你每一杯調酒的滋味與一起乾杯的時光。還有聲 音學程的各位，與燴飯一起吃過新竹更多的美食，與可柔、小高等人一同看電影、 吃飯唱歌、度過刺激的聖誕趴與經歷許多有趣的事情~ 感謝碩二的你們， 一起分享 生活樂事，一起討論人生夢想，一同歡笑(但沒有一同流淚 XD)，讓我碩士的生涯 增添更多豐富的色彩。 最後要感謝的，是我的父親林憲清先生，母親王寶秀女士，與妹妹冠婷。在 這二十幾載的歲月中，能夠忍受我的不成熟與不懂事，無怨無悔的栽培我。感謝你 們的支持，分享我的喜悅與憂愁，給我人生這門功課許多珍貴的教誨，並提供了不 虞匱乏的生活與強勁的後盾，讓我在學習的路程上都能順遂平穩，成就今天的自己， 讓我盡情揮灑、創造自己的人生。 在此，謹以本論文獻給我最摯愛的雙親。

v

Contents

摘 要 ... i Abstract ... ii 誌謝 ... iii Contents ... v

List of Tables ... vii

List of Figures ... viii

Chapter 1 Introduction ... 1

1.1 Motivation ... 1

1.2 Previous Work ... 2

1.3 Contribution ... 5

1.4 Thesis Organization... 6

Chapter 2 Review of Related Techniques... 7

2.1 Introduction of Basic Ultrasonic Range Measurement ... 7

2.1.1 The Physics ... 7

2.1.2 Envelope Derivation ... 8

2.2 Model of Ultrasonic Echo Envelope ... 9

2.3 Previous Method of TOF Estimation ...11

2.3.1 Threshold Method ...11

2.3.2 Two Maximums Algorithm ...12

2.4 Newton-Ralpshon with Levenberg-Marquardt Modification ...15

2.4.1 Original Newton Ralphson Recursive Formula ...15

2.4.2 Levenberg- Marquardt Modification (LM algorithm) ...15

2.5 Extended Kalman Filter ...16

2.6 Least Square Method ...19

Chapter 3 Proposed Method for Time of Flight Estimation ... 20

vi

3.2 Detection of Desired Region within Echo for Curve Fitting ...22

Chapter 4 Object Localization and Tracking in Multi-Channel ... 25

4.1 The Least Square Method ...25

4.2 Newton-Ralpshon with Levenberg-Marquardt Modification ...27

4.3 Discrete Extended Kalman Filter ...27

Chapter 5 Experiment Result and Comparison ... 29

5.1 Structure of Touch Screen Platform ...29

5.2 TOF Estimation ...32

5.2.1 Experimental Results ...32

5.2.2 Comparison and Discussion ...36

5.3 Objection Localization and Tracking ...38

5.3.1 Experimental Results ...38

5.3.2 Comparison and Discussion ...50

Chapter 6 Conclusions and Future Work ... 59

Appendix ... 61

vii

List of Tables

Table 5.2-1 : Marker pen, comparison of TOF standard deviation (unit: cm) ... 33

Table 5.2-2 : Human’s index finger, comparison of TOF standard deviation (unit: cm) ... 35

Table 5.3-1 : Marker pen, standard deviation comparison of target coordinate (x, y) (unit: cm) ... 38

Table 5.3-2 : Marker pen, mean of target coordinate (x,y) among different methods (unit: cm) ... 39

Table 5.3-3 : Human’s index finger, standard deviation comparison of target coordinate (x, y) (unit: cm) ... 41

viii

List of Figures

Figure 1.2-1 : The application of ultrasonic parking sensor ... 3

Figure 1.2-2 : System structure of [6] and geometry analysis of classifying the target into (b)(c)(d) ... 4

Figure 1.2-3 : The hand-writing message system based on smartphone ... 4

Figure 1.2-4 : System structure of digital pen (transmitter) and receivers on writing panel ... 5

Figure 2.1-1 : The basic principle of ultrasonic range measurement ... 7

Figure 2.1-2 : Envelope of echo (blue line) and the starting point t0 ... 8

Figure 2.2-1 : Envelope of echo from general envelope model ... 10

Figure 2.2-2 : Envelope of echo from subtraction between two decayed exponential terms... 11

Figure 2.3-1 : The envelope of echo (blue line) and TOF measurement by thresholding. ... 12

Figure 2.3-2 : Parameters in two maximums algorithms ... 14

Figure 2.5-1 : A complete picture of extended Kalman filter ... 19

Figure 3.2-1 : Ideal shape of envelope ... 23

Figure 3.2-2 : Double peaks phenomenon ... 23

Figure 3.2-3 : Curve fitting in desired region of echo ... 24

Figure 5.1-1 : Ultrasonic touchscreen platform ... 29

Figure 5.1-2 : Real experiment environment of ultrasonic touchscreen platform with target as human’s finger ... 30

Figure 5.1-3 : Target as marker pen ... 30

Figure 5.1-4 : Target as human index finger ... 31

Figure 5.2-1 : Marker pen, comparison of std of different methods among 5 channels. ... 32

Figure 5.2-2 : Human’s index finger, comparison of std of different methods among 5 channels ... 34

Figure 5.2-3 : Comparison between 2 model ... 37

Figure 5.3-1 : Object localization comparison ... 40

Figure 5.3-2 : Object localization comparison ... 43

Figure 5.3-3 : Object tracking comparison ... 44

Figure 5.3-4 : The EKF states change of x and y direction from Figure 5.3-3 (c)... 45

Figure 5.3-5 : Object tracking comparison ... 46

Figure 5.3-6 : The EKF states change of x and y direction from Figure 5.3-5 (c)... 47

Figure 5.3-7 : Object tracking comparison ... 48

Figure 5.3-8 : The EKF states change of x and y direction from Figure 5.3-7 (c)... 49

Figure 5.3-9 : Measurement data of Case3 (3) ... 51

Figure 5.3-10 : Object tracking comparison ... 52

Figure 5.3-11 : The EKF states change of x and y direction from Figure 5.3-10 (b) ... 53

Figure 5.3-12 : Object tracking comparison ... 54

Figure 5.3-13 : The EKF states change of x and y direction from Figure 5.3-12 (b) ... 55

Figure 5.3-14 : Object tracking comparison ... 56

Figure 5.3-15 : The EKF states change of x and y direction from Figure 5.3-14 (b) ... 57

1

Chapter 1 Introduction

1.1 Motivation

Since the mid-21th century, the market of touchscreen panels grows very fast, and the related techniques are becoming popular as well. There are a variety of touchscreen technologies that have different method of sensing touch [16], including resistive, capacitive, infrared projection and optical image. The most common disadvantage is that the cost would be relative high when applying these techniques to large panel. The cost of optical image is low but it would be affected by the light intensity in the current environment.

Those problems mentioned above can be avoided when applying acoustic wave technology, which uses ultrasonic wave that pass over the touchscreen panel. Only few sensors are required on the panel so that it wouldn’t cost much, and the ultrasonic wave would not be interfered by the variation of light intensity.

However, ultrasonic wave touchscreen panel can be damaged by outside elements, target with complicated shape (like human’s hands) or the contaminants on the screen surface, which would interfere with the functionality of the touchscreen. Previous works of ultrasonic range measurement deal with large obstacle detection and localization [1-2] or distant objects when the echo duration is negligible compared with the travel time [1, 3-5]. The measurement method based on those applications mentioned above might not robust enough in the scenario of touchscreen platform.

Therefore, this thesis develops a strategy for more accurate and stable ultrasonic range measurement and object localization on touchscreen platform, which is a noisy environment and the echo waveform might be changing consistently or seriously distorted.

2

1.2 Previous Work

The common goal of ultrasonic range measurement is to estimate the Time of Fight

(TOF) among all the previous works.

Several ways of TOF estimation have been provided. The threshold method is the simplest way of measuring TOF, where the TOF is determined when the amplitude of echo first exceeds the threshold value [1, 2]. In case that only one threshold value is not robust enough to noise, the double threshold method [2] is presented in which two-points are fitted to the rising edge of the ultrasonic echo envelope with parabolic approximation. Whichever threshold methods mentioned above would always have a biased problem on TOF, the optimum correlation detection method [1, 2] could provide unbiased estimation by using matched filter that contains a replica of the echo waveform and is employed to determine the most probable location of the echo in the received signal. However a large number of templates for the expected signal must be stored for the correlation operation due to the variation of the echo waveform, which might consume a large computation of computer.

Curve fitting is another popular method for TOF estimation, which requires a mathematical model for certain part of echo envelope. [1] provides a basic curve fitting method by using nonlinear least-squares with parabolic curve to fit the onset of the ultrasonic echo. Other envelope models are used for fitting the entire echo. [3, 4] apply similar analytical model for the envelope with different approach. [3] proposes a two maximums algorithm for TOF estimation, which linearizes the model and derive the TOF along with the rising edge of envelope. Another way for finding TOF is by Kalman Filtering as in [4], which estimates the shape factors of the echo envelope and could assure to reduce bias and uncertainty in critical TOF measurement, but since the algorithm tracks sample by sample, a time delay issue might be raised in real time because of the large amount of raw data with high sampling rate.

3

One of the common applications of previous works is sonar. For example the ultrasonic parking sensors [15] use the very basic ultrasonic ranging principle to detect the distance between cars and obstacles.

Figure 1.2-1 :The application of ultrasonic parking sensor

Another sonar-like application is in robotics field, [6] presents a sonar array (Figure 1.2-2) in mobile robotics for localization and mapping of indoor environment. The optimal echo arrival time is estimated from the maximum cross-correlation of the echo with the templates. Then the geometry analysis is provided to classify the target into several types such as planes, corners, edge or unknown based on the array structure they proposed. Similar application can be investigated in [1-4], and usually the actual distance of reflectors ranges from 0.5m to 3.5m in these applications.

4

The nearby target localization can be found in [7, 8]. [7] develops a hand-writing system on smartphone. There is an ultrasonic transmitter equipped on digital pen, and receivers are on the edge of the writing panel. By measuring the distance between the signal pen and the, the coordinates of pen can be calculated and transmitted to the smartphone (Figure 1.2-3, Figure 1.2-4).

Figure 1.2-3 : The hand-writing message system based on smartphone

(a) (b)

(c) (d)

Figure 1.2-2 : System structure of [6] and geometry analysis of classifying the target into (b)(c)(d) (a) The structure of [6] (b) planes (c) corners (d) edge

5

Figure 1.2-4 : System structure of digital pen (transmitter) and receivers on writing panel

1.3 Contribution

The contribution can be divided in two parts. First, a method of TOF estimation under noisy environment is proposed. We use the double exponential model in [8] to characterize the echo, and fit the envelope curve by the Newton’s method [9] to derive the parameters of the model. An optimal fitting region is also located based on the idea features of the model, which avoid fitting on the other distorted measurement data and reducing the bias of estimation. Secondly, a discrete extended Kalman filter is design to estimate the target coordinate, which considers the interference of TOF estimation. A strategy for outlier issue in measurement is also provided with modification on update process in EKF, which can efficiently detect and reject the outlier and increase the robustness of target tracking. The experimental results have shown that the proposed method for ultrasonic range measurement provides stable TOF estimation with tolerable bias, and the performance of target localization and tracking is also improved under application of touchscreen panel.

6

1.4 Thesis Organization

Chapter 2 starts with a review of related techniques used in this project.

Chapter 3 describes the proposed method for TOF estimation, from the mathematical conception to dealing with the real cases of echo waveform.

Chapter 4 introduces three methods for target coordinate estimation after deriving the TOF measurement from Chapter 3 and in

Chapter 5 the experimental result is presented under different scenarios to evaluate the proposed methods for estimation of TOF and target coordinate comparing to the other methods.

Finally Chapter 6 concludes the thesis and provides suggestions and perspectives of future work.

7

Chapter 2 Review of Related Techniques

2.1 Introduction of Basic Ultrasonic Range Measurement

2.1.1 The Physics

The basic principle ultrasonic range measurement (Figure 2.1-1) is to let a transmitter send a pulse at time t_s, and measure the time t0 when echo arrives at receiver,

the distance between two sensors is then obtained by (t0 – ts) × Sound velocity, (t0 – ts) is

so called the Time-of-Flight.

Figure 2.1-1 : The basic principle of ultrasonic range measurement

t0 ts

8

2.1.2 Envelope Derivation

To find the exact starting point t0, the envelope is usually extracted first (Figure 2.1-2)

There are many ways to derive the envelope. The method we use here is demodulation with double sine [10].

Let the function of received signal be

( ) ( )sin( )

R t A t wt (2.1-1)

where A(t) is the envelope, wis the high frequency, and  is the low frequency. First R(t)is multiplied by sin(wt)and cos(wt)

1 ( ) sin( ) sin( ) 1 1 ( )( cos( ) cos(2 )) 2 2 x A t wt wt A t wt         1 ( )cos( ) 1 ( )cos(2 ) 2A t  2A t wt     (2.1-2) t0

9 2 ( ) sin( ) cos( ) 1 1 ( )( sin(2 ) sin( )) 2 2 x A t wt wt A t wt         1 ( ) sin( ) 1 ( ) sin(2 ) 2A t  2A t wt     (2.1-3)

so we can design a low pass filter with cut-off frequency near  to filter out w

x₁ LPF ₁ 1 ( )cos( ) 2 z  A t  (2.1-4) x₂ LPF ₂ 1 ( )sin( ) 2 z  A t  (2.1-5) Then the envelope can be extracted by

2 2 1 2

( ) 2

A t  z z (2.1-6)

2.2 Model of Ultrasonic Echo Envelope

The general form of envelope model [4] is expressed as

0 ( ) 0 0 ( , ) ( ) t t T t t h t A e T      x (2.2-1)

where x=[ A₀, , T,t₀], A₀accounts for the echo amplitude, and Tare distinct to the specific ultrasonic transducer, and t₀is the desired TOF. The model characterizes the envelope waveform by a parabolic term and an exponential decay term.

10

The other model we use here is the double exponential model [8]:

0 0

( ) ( ) 0

( , ) ( t t t t )

h x t V e  e  (2.2-2) wherex=[ V₀,  ,  ,t₀], V₀ is amplitude,  and  are the decay factors (notice that  must larger than  ). The double exponential model is so called because it characterizes the envelope by two exponential terms (Figure 2.2-2).

Figure 2.2-1 : Envelope of echo from general envelope model with A₀=0.1, =3, T =50.

11

Note that the main difference between the general model and double exponential model is that they characterize the rising edge (the reign from TOF to peak) of echo differently. General model characterizes the rising edge as parabolic term whereas double exponential characterizes as concave downward curve.

2.3 Previous Method of TOF Estimation

A variety of TOF estimation methods can be investigated in [1]. In this section we introduce some common methods of measuring TOF.

2.3.1 Threshold Method

The simplest way of measuring TOF is thresholding. The TOF is the time t0at which

the echo amplitude first exceeds a preset threshold level  (Figure 2.3-1).

Figure 2.2-2 : Envelope of echo from subtraction between two decayed exponential terms with V₀=1, =0.005, =0.02.

12

Assuming noise is Gaussian distribution  is usually set equal to 3-5 times the noise standard deviation.

2.3.2 Two Maximums Algorithm

The two maximums algorithm [3] uses the analytical model of ultrasonic envelope, which is similar to (2.2-1): 0 ( ) 0 0 ( ) ( )n t t A t  A tt e  (2.3-1) where t₀is TOF, A₀,,and nare experimental constants based on a set of experimental signal. The algorithm is based on two characteristic instants: the maximum amplitude and maximum slope, which can be calculated by taking the 1st and 2nd derivate of (2.3-1)

max 0 n t t    (2.3-2) * 0 n n t t     (2.3-3)

Figure 2.3-1 : The envelope of echo (blue line) and TOF measurement by thresholding.

0

t

13 Let h 0 max 0 * t t t t    1 1 n   (2.3-4)

where his the ratio of t0 to *

t and t0totmax. If we let n=2 (assuming the 2

nd

order curve), then h= 0.2929, and t₀can be estimated as

* max 0 1 t ht t h    (2.3-5)

therefore we have to estimate *

t and t_maxfirst.

For the estimation of maximum slope instant *

t , the procedure consists in calculating the amplitude relation ( r ) between V_maxand the maximum slope amplitude *

V .Taking equation model (2.3-1) and the temporal relation (2.3-4), we can find

* max 1 (1 )n n V r e V n    (2.3-6) now since n=2, r =0.3529, * V =0.3529V_max So that *

t can be estimated by interpolation supposing that the envelope signal is as straight line in a short interval around *

V .

To estimatet_max , we first choose the instant corresponding to the 0.8V_max(t_0.8max) where it’s value is close to V_maxbut with higher slope, and can estimate t_maxmore accurate ( t_0.8maxcan be obtained by interpolation). Then t_maxis calculated by

* max * max 8 . 0 max max max 8 . 0 max 8 . 0 8 . 0 V V t t V V t t     

14 max 8 . 0 * max * max 8 . 0 max max 8 . 0 ) ( 2 . 0 t V V t t V t     =offset + t_{0.8 max} (2.3-7)

The TOF t₀can be obtained using (2.3-5) along with (2.3-6) and (2.3-7). offset

15

2.4 Newton-Ralpshon with Levenberg-Marquardt Modification

In this section we discuss the nonlinear least-squares method [9] which is employed to fit a curve at the onset of ultrasonic echo in order to produce the unbiased TOF measurement.

2.4.1

Original Newton Ralphson Recursive Formula

Define objective function f (x), and we want to find ˆx such that

 

ˆ arg min x

x f x (2.4-1)

We can obtain a quadratic approximation to the twice continuously differentiable function using the Taylor series expansion of f (x) about the current state ( )k

x , neglecting terms of order three or higher.

f (x)  f ( ( )k x ) + (x - ( )k x )T g( )k + 1 2(x - ( )k x )T F x( ( )k )(x - ( )k x ) q(x) (2.4-2) where ( )k g = f ( ( )k x ), ( ) ( k ) F x =2 f ( ( )k x ) when 0=q(x)= ( )k g + ( ) ( k ) F x (x - ( )k x )，q achieves a minimum at (k 1) x  = ( )k x - ( ) 1 ( k ) F x  g( )k (2.4-3) (2.4-3) is the basic Newton Ralphson recursive formula.

2.4.2 Levenberg- Marquardt Modification (LM algorithm)

A potential problem for the Newton method is that if the Hessian matrix ( )

( k )

F x is

not positive definite, then the search direction ( )k

d = - ( ) 1

( k )

F x  g( )k may not point in a descent direction, the Levenberg- Marquardt Modification of Newton’s Algorithm can ensure that the search direction is descent direction by modifying Newton Ralphson recursive formula as (k1) x  = ( )k x - ( ) 1 ( (F xk ) kI)   ( )k g , _k 0 (2.4-4)

16

The idea underlying the Levenberg- Marquardt Modification is as follows.

Consider a symmetric matrix F, which may not be positive definite. Let ₁,..._n be eigenvalues and v₁,...v_n be corresponding eigenvectors of F，where the eigenvalues are real but may not be positive.

Next consider G = F+I, where  0,

G v_i = (F+I)v_i

= Fvi+I vi = _iv_i+ v_i

= (_i+)v_i, (2.4-5) therefore   ₁ ,... _n  are also eigenvalues of G with corresponding eigenvectorsv_i. When is sufficient large, then all of the eigenvalues of G are positive and G is positive definite.

Accordingly if the parameter _k in Levenberg- Marquardt Modification of Newton’s Algorithm is sufficient large, then the search direction ( )k

d = - ( ) 1

( (F xk ) kI) 

 ( )k

g would always points to descent direction (Theorem9.2 in [9]).

2.5 Extended Kalman Filter

The Kalman filter, originally proposed in [5], deals with the general problem of trying to estimate the state of a discrete-time controlled process that is governed by a

linear stochastic difference equation and also with a linear measurement equation.

However if the process to be estimated or the measurement relationship to the process is non-linear, a Kalman filter that linearizes about the current mean and covariance is referred to as an extended Kalman filter or EKF [11].

Assume that state equation

1 1

( )

k k k k

17

with a measurement equation

( )

k k k

z h x v, (2.5-2)

where w_k ~N(0,Q_k)and v_k ~ N(0,R_k)

The non-linear function in the difference equation (2.5-1) relates the state at the previous time step k -1to the state at the current time step k. It includes as parameters any driving function u_k and the zero-mean process noise w_k. The non-linear function in the measurement equation (2.5-2) relates the state to the measurement.

To begin with, we rewrite the governing equation that linearize an estimate about (2.5-1) and (2.5-2) 1 1 1 ( ) + k k k k k x x A x _ xˆ _ Ww_ (2.5-3) ( ) k k k k k z z H x x Vv (2.5-4) where

• xk and zk are the actual state and measurement vectors.

• xk and zk are the approximate state and measurement vectors from (2.5-1) and (2.5-2). • xˆk1 is an a posteriori estimate of the state at step k.

• The random variables w_k1 and v_k represent the process noise and measurement noise. • A is the Jacobian matrix of partial derivatives of with respect to x, that is

_{[ ]} [ ] [ ] ( 0) , ˆ , , i i j k k j f A x u x   

• W is the Jacobian matrix of partial derivatives of with respect to w, _{[ ]} [ ] [ ] ( 0) , ˆ , , i i j k k j f W x u w   

• H is the Jacobian matrix of partial derivatives of with respect to x _{[ ]} [ ] [ ] ( 0) , , i i j k j h H x x   

18 [ ] [ ] [ ] ( 0) , , i i j k j h V x v   

Note that for simplicity in the notation we do not use the time step subscript with the Jacobian matrices A, W, H, and V, even though they are in fact different at each time step. After defining the above parameters, we can now re-derive the complete EKF equations shown below. For the EKF time update equation (Prediction step):

k xˆ f x(ˆ , ,_k1 u w_{k k1}) (2.5-5) 1 1 T T k k k k k k k P  A P A_ W Q W_ (2.5-6) For the EKF measurement equation (Correction step)

1 ( ) T T T k k k k k k k k k K P H H P H V R V  (2.5-7) k xˆ = xˆ_kK z_k( _kh x v(ˆ ,_k _k)) (2.5-8) ( ) k k k k P  I K H P- (2.5-9)

As with the basic discrete Kalman filter, the time update equations (2.5-5) and (2.5-6) project the state and covariance estimates from the previous time step k-1 to the current time step k and the measurement update equations in (2.5-7)~(2.5-9) correct the state and covariance estimates with the measurement z_k. Now that we can summarize the operation of the EKF as shown in Figure 2.5-1

19

2.6 Least Square Method

Consider a system of linear equations

 Ax b (2.6-1) where m n R   A , n1 R 

x , bR mn, n, and rank An. If bis not belongs to the range of

A , say bR A( ) , then the system of equations is said to be inconsistent or overdetermined. In this case there would be no solution to the above set of equation. Therefore, the vector *

x that minimizes Ax b 2is given by

 

* T 1 T

x (A A) (A b) (2.6-2)

where *

x is called the least square estimator.

20

Chapter 3 Proposed Method for Time of Flight Estimation

3.1 Curve Fitting Using Nonlinear Optimization Method

Here we use the envelope model from (2.2-1) or (2.2-2) for curve fitting using Newton-Ralpshon with Levenberg-Marquardt Modification. The result of TOF estimation using these two models would be compared in chapter 5.

Since the envelope model is nonlinear, we construct the following nonlinear least square problem for Newton-Ralpshon with LM Algorithm.

Let state x = [ , ]t₀ v and envelope model be h(x, t), where t₀ is the TOF to be estimated

and v the other parameters of the model. The measurementy[ ,...y₁ ym]is the original

envelope data in each frame. Consider the following objective function: Minimize 1 ( m i i y  



h(x, t_i) 2 ) (3.1-1) let r_i(x) = y_i  h(x, t_i), defining r = [ ,...,r₁ r_m]T, then objective function can be written as

f (x)= r (x)T _{r (x) (3.1-2)} To apply Newton’s Method, we need to compute the gradient  f ( x) and the Hessian F( x) of f . the j-th component of f (x) is ( f ( x) )_j= i f f x   (x) = 2 1 m i i r 



(x) i j r x   (x) (3.1-3)

denote Jacobian matrix of r by

J(x) = 1 1 1 1 ( ) ( ) ( ) ( ) n m m n r r x x r r x x     _{ }      _{ }    _{ }    x x x x (3.1-4)

21

then the gradient of f can be represented as

 f ( x) = 2 J(x)T_{r (x) (3.1-5)} for (k,j)th component of Hessian matrixF(x)

2 k j f   x x (x) = k ( j f   x x (x)) = 1 (2 m i i k r   x



( x) i j r  x (x)) = 1 2 m i



( i k r  x (x) i j r  x (x) + ri(x) 2 i k j r   x x (x)) (3.1-6)

let (k,j)th component of S(x)be 1 m i



ri(x) 2 i k j r   x x (x), (3.1-7)

then we can rewrite the Hessian matrix as

F(x) = 2(J(x)TJ(x)+ S(x)) (3.1-8)

therefore the recursive formula for Newton method applied to the nonlinear least squares problem is given by

_x(k1)_=x( )k _{- ( J(x} ( )k

)TJ(x ( )k )+ S(x ( )k ) 1

) J(x ( )k )Tr (x ( )k ) (3.1-9) Usually S(x) involving the second derivatives of r (x) can be ignored because its components are negligibly small. In some application Newton Method reduces to what it commonly called Gauss-Newton method:

(k1) x = ( )k x - ( J(x ( )k )TJ(x ( )k ) 1 ) J(x ( )k )Tr (x ( )k _{) (3.1-10)} finally we rearrange (3.1-11)and (2.4-4) with LM algorithm

(k1) x = ( )k x - ( J(x ( )k )T_J(x ( )k ₎ kI   1 ) J(x ( )k )T_{r (x} ( )k _{) (3.1-11)} which is the ongoing recursive formula for TOF estimation.

22

3.2 Detection of Desired Region within Echo for Curve Fitting

Generally, when the maximum amplitude of the echo is detected, the target is assumed to be present and a TOF estimate is produced. However, echo waveform could be distorted, causing multiple peaks phenomenon and interfering with TOF estimation. Therefore, instead of fitting the whole envelope data, we locate a fitting window on desired peaks based on the mathematical attributes of envelope model.

For example, reconsider the model of (2.2-1)

which is only one peak value and the curve from peak to onset is monotonic decrease, i.e. no turning points. So that the ideal shape of waveform within fitting window should be one peak and the onset is monotonic decrease (Figure 3.2-1), otherwise it will cause big fitting error and affect the accuracy of TOF estimation (Figure 3.2-2). In both Figure 3.2-1 and Figure 3.2-2, the received waveform is blue line and the fitted model is red line.

0

ˆt is the estimated TOF from (3.1-11).

t

₀is the ground truth for TOF, derived from target coordinate which is measured manually. (t_max,V_max) is peak value of echo and (t*, *

V ) are chosen from two maximums algorithm, which are the instants that characterizes the onset of echo. The size of fitting window is defined from t* to t_max+ n, where n is any arbitrary number.

23

We construct the following process to solve the multi-peaks problem (Figure 3.2-2): (1) Set the first detected maximum amplitude as initial value (t_max0, V_max0), and find the corresponding (t*, V*).

(2) In i-th iteration, check if the waveform from t_maxito t*is monotonic decrease, then go to (3).

(3) Execute the Newton’s method with the applied envelope model to obtain ˆt₀.

Through the above process we can find the suitable fitting window, and then get the best fitting result. (Figure 3.2-3)

Figure 3.2-2 : Double peaks phenomenon 0 0 (t_max ,V_max ) t V*, * ( ) desired echo 0 ˆt t0

Figure 3.2-1 : Ideal shape of envelope

t V* * , ( ) 0 t 0 ˆt Fitting window (t_max,V_max) i i t_max ,V_max ( ) Turning point

24

0

ˆt t₀

Figure 3.2-3 : Curve fitting in desired region of echo

i i t_max ,V_max ( ) t V* * , ( )

25

Chapter 4 Object Localization and Tracking in

Multi-Channel

There are several ways to obtain the target coordinate when the TOFs form each channel is derived. In this chapter we construct three methods for coordinate estimation, and the result would be compared in chapter 5.

4.1 The Least Square Method

Target coordinate can be calculated by linear equations based on the round trip from transmitter to receivers.

Let s

v be the sound speed, N be the total number of receivers,

[ ]T

Ri  xRi yRi

x : the coordinate of the i-th receiver,

[ ]T

T  xT yT

x : the coordinate of the transmitter,

[x y]T 

x : the coordinate of target,

l_i : the round trip distance between the transmitter, target, and the i-th receiver, i

 be the time of flight between the transmitter, target, and the i-th receiver , i = 1~N.

d_T : the distance between the transmitter and the target,

d_Ri : the distance between the i-th receiver and target, we have 2 2 |x x _Ri| x xT 2x xT _Ri x x_RiT _Ri d_Ri 2 2 |x - x_T | x x - 2x x + x xT T _{T T}T _T d_T d_T + d_Ri = l_i 2 2 2 2 T T Ri Ri i d  d d d l 2 2 2 2 T Ri i T Ri d d  l d d 2 2 _{T T T} 2 _{Ri Ri Ri} l_Ri 2d d_{T Ri}        T T T T T T x x x x x x x x x x x x

26 2 2x xT 2x xT( _T x_Ri)x xT_{T T} x xT_{Ri Ri}  l_i 2d d_{T Ri} (4.1-1) replace d_Ri by l_i d_T 2 2x xT 2x xT( _T x_Ri)x xT_{T T} x x_RiT _Ri  l_i 2d l_T(_i d_T) 2 2 2x xT 2x xT( _T x_Ri)x x_TT _T x xT_{Ri Ri}  l_i 2d l_{T i} 2d_T 2 2x xT( _T x_Ri)x x_TT _T x x_RiT _Ri  l_i 2d l_{T i} replace l_i by v_{s i} we get 2 2x xT( _T x_Ri)x x_TT _T x xT_{Ri Ri}  l_i 2d v_T( _{s i} ) (4.1-2) Therefore (4.1-2) can be written in matrix form and the least square problem is constructed as

Aβ = b (4.1-3)

where

The target coordinate can be estimated by

ˆ T -1 T β = (A A) A b (4.1-4) 1 1 2 1 1 1 2 2( ) 2 2( ) 2 ( ) ( ) T R s T RN s N T T T s T T N N s N v v d v v              _                     _{ }    T T T T T T T T x x A x x x β x x x x b x x x x

27

4.2 Newton-Ralpshon with Levenberg-Marquardt Modification

The Newon’s Method with LM algorithm can also be used in target coordinate estimation. Assume N receivers, we first define the state as target coordinate x =( , )x y , the measurement value y_i denotes the TOFs from the i-th receiver (i = 1~N), we now fit the measurement data by the round trip equation from transmitter (x_T,y_T) to target and to the

i-th receiver (x_Ri,y_Ri). 2 2 2 2 1 ( ) ( ( ) ( ) ( ) ( ) ) i T T Ri Ri s h x x y y x x y y v         x (4.2-1)

Now that we can construct the objective function similar to (3.1-1) Minimize 1 ( m i i y  



h x_i( ))2 (4.2-2) Applying the recursive formula from (3.1-11), we can finally obtain the target coordinate.

4.3 Discrete Extended Kalman Filter

For the object localization and tracking, the EKF was used to estimate the position

( , )x y and velocity ( , )x y of the target. The state equation is directly derived from the motion model as

(4.3-1)

Let the state x_k [x y_k, _k,,x y_k, _k]. Then the measurement equation for the i-th receiver is given by

( )

i

k i k k

z h x v (4.3-2)

where h_i( ) is the same model as (4.2-1), w_k ~ N(0,Q_k) and v_k ~ N(0,R_k).

1 1 1 1 1 0 0 0 1 0 0 0 1 0 0 0 0 1 k k k k k k k k k x t x y t y x x y y                _  _                 w

28

When the TOF is obtained from each channel, it is possible that outliers would happen when the echo is seriously distorted under conditions such as target with complicated shape or when target is moving, and affect the performance of EKF. A reliable and efficient outlier detection method is provided by [12]. Let the prediction step in the proposed EKF be

ˆ_k

x Axˆ_k1,

where A is the system matrix of (4.3-1).

So that we can derive the predicted measurement of i-th receivers by (4.2-1) as

ˆ ( )

i

k i k

z h x 

Then the update process from (2.5-7) to (2.5-9) is modified as

1 1 ( T ) k k k k k k     S H P H R w (4.3-3) T k k k k   K P H S ˆ_k ˆ_k _k( _k  _k) x x K z z ( ) k k k k    P I K H P

A weight w_kis introduced as a divisor of R_kin (4.3-3), and is derived from below 0.5 ( ) ( ) k T k k k k k a b      w z z R z z (4.3-4)

which reveals that if the prediction error in z_kis so large that it dominates the denominator, then the weight w_k of that data sample will be very small. As this prediction error term in the denominator goes to ∞, w_k approaches 0. If z_khas a very small weightw_k, then S, the posterior covariance of the residual prediction error, will be very small, leading to a very small Kalman gain K_k . In short, the influence of the data sample z_k will be downweighted when predicting ˆk



x , the hidden state at time step k .Here a = b=1 as suggested in [12].

29

Chapter 5 Experiment Result and Comparison

5.1 Structure of Touch Screen Platform

The architecture of touch screen platform using ultrasonic sensors is shown in Figure 5.1-1 and Figure 5.1-2. There are one transmitter (Red one) and five receivers (Blue one) equipped on the edge of a 24 inches screen, with U shape sensor shelf (The ultrasonic sensors here are all transceivers so that they can act as transmitter or receiver, the type of all sensors is 400PT160, with center frequency at 40kHz. The detail of specification can be checked on the website of Pro-Wave Electronic Corp [17]). An NI data acquisition (DAQ) is connected between PC and each sensor, and the transmitter is driven by PC with a rectangular burst consisting of 8 cycles. When receivers measure echoes reflected from target, the analog raw data would be recorded by PC through DAQ.

Demo Screen

Figure 5.1-1 : Ultrasonic touchscreen platform

NI Data Acquisition 6366

PC

target

R2 R3

T

R5

R4 R1

30

In section 5.2 and 5.3, there are three cases to be tested, which are Case1:

Target as marker pen standing vertically to the touchscreen at measured coordinates.

Figure 5.1-3 : Target as marker pen

31

Case2:

Target as human’s hand with index finger pointing vertically to the touchscreen at measured coordinate.

Figure 5.1-4 : Target as human index finger

Case3:

Tracking on human’s hand with index finger pointing vertically to touch screen. Several trajectories are tested.

32

5.2 TOF Estimation

5.2.1

Experimental Results

In this section we compare the standard deviation of TOF estimation in Case1 and Case2, where the targets are all stationary. The two maximums algorithm we use here is applied to the same fitting window as in Newton’s method. In Case 1 and Case 2, there are 700 testing frames, and 300 testing frames in Case 3. Note that here we compare the standard deviation of TOF in centimeter unit for analyzing convenience.

Case1:

33

Table 5.2-1 : Marker pen, comparison of TOF standard deviation (unit: cm) TOF

Estimation Method

Threshold Two Maximums

Newton’s method w/ general model Newton’ method w/ double exponential Target Coordinate #of channel (14, 37) 1 0.370 0.209 1.107 0.232 2 3.667 0.761 1.302 0.755 3 4.275 1.576 2.069 1.485 4 0.363 0.193 0.682 0.242 5 0.395 0.210 0.869 0.262 (18, 37) 1 0.443 0.180 0.580 0.222 2 3.268 1.213 1.749 1.125 3 4.836 2.428 4.007 2.270 4 0.317 0.167 0.783 0.193 5 0.422 0.231 0.777 0.262 (14, 41) 1 1.661 0.65 0.675 0.318 2 4.471 0.522 1.042 0.473 3 2.789 0.674 1.115 0.669 4 0.871 0.185 0.760 0.219 5 0.444 0.251 0.767 0.317 (18, 41) 1 9.079 1.042 1.455 0.998 2 10.791 1.906 1.936 1.201 3 4.641 0.482 0.932 0.480 4 6.761 0.271 0.716 0.325 5 4.599 0.149 0.745 0.175

34

Case 2:

35

Table 5.2-2 : Human’s index finger, comparison of TOF standard deviation (unit: cm) TOF

Estimation Method

Threshold Two Maximum

Newton’s method w/ general model Newton’ method w/ double exponential Target Coordinate #of channel (14, 37) 1 1.874 3.052 2.299 1.644 2 4.305 0.921 1.566 1.266 3 4.944 1.418 1.904 1.492 4 1.183 1.488 1.728 0.831 5 2.895 1.731 2.165 1.524 (18, 37) 1 1.103 2.019 1.934 0.755 2 3.243 4.487 2.852 2.399 3 5.068 2.207 2.186 1.807 4 1.048 0.768 1.440 0.736 5 1.171 0.601 1.485 0.657 (14, 41) 1 5.394 0.811 1.851 0.751 2 10.473 3.263 3.572 3.147 3 4.790 1.591 2.052 1.770 4 5.778 1.218 1.651 1.009 5 4.293 4.250 4.642 3.490 (18, 41) 1 2.858 2.730 2.518 1.880 2 6.529 3.676 3.826 3.237 3 4.084 1.444 2.717 1.815 4 3.530 0.826 1.757 0.883 5 2.119 0.874 1.927 0.889

36

5.2.2 Comparison and Discussion

From Figure 5.2-1 and Figure 5.2-2, we observe that the threshold method can have less variation in TOF estimation when target is stationary and with smooth surface. Echo signal under this case is usually stable. However since the threshold value is determined arbitrary, the result of estimation could be seriously affected by disturbance wave with amplitude larger than the threshold value whether the target is marker pen or human’s hand, and cause several outliers.

The estimation result of two maximums algorithm is relative stable and accurate than threshold method since it applies linear interpolation based on envelope model in (2.3-1) on the rising edge to estimate TOF, and the rising edge is also located in desired echo region derived from the method in section 3.2. However the way of linear interpolation is very sensitive to the variation of slope on the rising edge, causing the corresponding interference to measured TOF.

Comparing Newton’s method with the two models (Figure 5.2-3), the general echo model characterizes the actual envelope better than the double exponential model, so that the residual of curve fitting using (2.2-1) would be smaller than (2.2-2). However, general echo model would be more sensitive to the variation of the measured echo signal since it fits the onset of echo well, yielding the interference to TOF. On the other hand, although there would be a little bias of TOF estimation from the double exponential model, the rising edge of the model is less sensitive to the variation of echo, which is consistent to the experiment result in [8]. Hence we conclude that the method of Newton’s optimization with double exponential model provides much more stable TOF estimation than other methods, and the derived TOF data is used by all cases in object localization and tracking in the section 5.3.

37

*From Figure 5.2-3 it is observed that since the general model fits the rising edge better than the double exponential model, so that the estimated TOF would be much more sensitive to the variation of slope on rising edge although the residual of curve fitting is smaller than double exponential model.

Figure 5.2-3 : Comparison between 2 model (a) curve fitting with general model (b) curve fitting with double exp. model (a) (b) 0 tˆ 0 tˆ

38

5.3 Objection Localization and Tracking

In this section three methods of deriving target’s coordinate are tested, including Least Square, Newton Ralphson’s method, and EKF. At first all the 5 channels are used among the three methods, then the outlier rejection issue would then be discussed in section 5.3.2.

5.3.1 Experimental Results

Case1:

Table 5.3-1 and Table 5.3-2 show the standard deviation and means of the target coordinate, we can see that the standard deviation of target coordinate is smallest using EKF.

Table 5.3-1: Marker pen, standard deviation comparison of target coordinate (x, y) (unit: cm) Target

Localization Method

Least Square Newton’s

method 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

39

Table 5.3-2 : Marker pen, mean of target coordinate (x,y) among different methods (unit: cm)

Target Localization

Method

Least Square Newton’s

method EKF Target Coordinate axis (14, 37) x 13.44 13.48 13.49 y 36.56 38.51 38.49 (18, 37) x 17.74 17.80 17.79 y 35.84 38.58 38.55 (14, 41) x 14.09 14.07 14.10 y 41.72 41.56 41.28 (18, 41) x 18.24 18.23 18.22 y 41.05 41.60 41.35

40

Figure 5.3-1 shows the XY-plot of the three methods with coordinate of marker pen at (18, 41).

(b) (a)

(c)

Figure 5.3-1 : Object localization comparison a) Least Square

b) Newton’s Method c) EKF

Note that the ground truth is measured manually.

R1

R2 R3

R1

41

Case2:

Table 5.3-3 and Table 5.3-4 show the standard deviation and mean of each target coordinate, we can see that the standard deviation of target coordinate is the smallest using EKF.

Table 5.3-3 : Human’s index finger, standard deviation comparison of target coordinate (x, y) (unit: cm) Target

Localization Method

Least Square Newton’s

method 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

42

Table 5.3-4 : Human’s index finger, mean of target coordinate (x, y) among different methods (unit: cm) Target

Localization Method

Least Square Newton’s

method EKF Target Coordinate axis (14, 37) x 13.72 13.64 13.72 y 35.69 37.97 37.89 (18, 37) x 17.70 17.70 17.70 y 36.65 37.94 37.75 (14, 41) x 13.42 13.41 13.56 y 42.11 41.60 41.23 (18, 41) x 17.87 17.83 17.86 y 40.72 41.39 41.00

43

Figure 5.3-2 shows the XY-plot of the three methods with coordinate of human’s finger at (18, 41) .

(a) (b)

(c)

Figure 5.3-2 : Object localization comparison a) Least Square

b) Newton’s Method c) EKF

Note that the ground truth is measured manually.

R1

R2 R3

R1

44

Case3: target as human’s index finger

(*Note that blue dots is the estimated coordinate, red line is the reference target

trajectory. Finger stopped at the starting point and end point for a while before and after moving.) (1) Target moves from (8, 40) to (20, 40), velocity of x direction is about 1cm/sec.

Figure 5.3-3 : Object tracking comparison a) Least Square b) Newton’s Method c) EKF (a) _(b) (c) R1 R2 R3 R1 R2 R3

45

46

(2) Target moves from (8, 36) to (20, 48), velocity of target is about 2 cm/sec.

Figure 5.3-5 : Object tracking comparison a) Least Square b) Newton’s Method c) EKF (a) (b) (c) R1 R2 R3 R1 R2 R3

47

48

(3) Target moves from (20, 36) to (20, 48), velocity of y direction is about 1cm/sec.

Figure 5.3-7 : Object tracking comparison a) Least Square b) Newton’s Method c) EKF (a) (b) (c) R1 R2 R3 R1 R2 R3

49

50

5.3.2 Comparison and Discussion

We first observe the least square problem from (4.1-3), the system matrix A that contains the range estimate would increase uncertainty of estimation. Newton’s method is better than least square method since the round trip model doesn’t contain any range estimation. However the Newton’s method only estimates the coordinate based on the current TOF data so that the estimation result would be directly affected by the measurement noise. Although the performance of Newton’s method and EKF are similar when target is stationary, the EKF can still provide more stable and smoother target localization and tracking since it estimates the target coordinate based on the previous state and inherently considers the interference of measurement.

Note that from Figure 5.3-4 and Figure 5.3-6 , the estimated velocity from EKF is close to the target moving velocity, but comparing the (x, y) estimation in Figure 5.3-5(c), tracking on x direction is delayed more than on y direction. It is because that the geometry of ultrasonic platform structure (Appendix and [13]) makes the variance of x direction bigger than y direction (we can also observe this phenomenon in Table 5.3-1 and Table 5.3-3 ). To solve this problem we simply set the process noise covariance of x direction smaller than that of y direction, which means that it would reduce the variance of the estimated x coordinate but the tradeoff is that the delay would increase when tracking on x direction. Finally from Table 5.3-2 and Table 5.3-4, it is observed that the estimated means of target coordinate are similar among three methods.

We then consider the outlier issue in case3, when target moves from (20, 36) to (20, 48), we observe that sometimes echo would be distorted seriously hence several outliers appear in measurement (Figure 5.3-9). The outliers cause a great effect on the tracking performance of EKF. In this section we use the outlier detection method mentioned in section 4.3 to improve the performance of tracking on target:

51

52

Case3: target as human’s index finger

(3) Target moves from (20, 36) to (20, 48), velocity of y direction is about 1cm/sec

Figure 5.3-10 : Object tracking comparison a) single EKF

b) EKF with outlier rejection

53

54

(1) Target moves from (8, 40) to (20, 40), velocity of x direction is about 1cm/sec

Figure 5.3-12 : Object tracking comparison a) single EKF

b) EKF with outlier rejection

55