Improvement of Ultrasound-based system using sine wave detector and CAN network

This paper presents an improved indoor localization system based on RF and ultrasonic signal which we named SNSH system. This system composes of a transmitter mounted in a mobile target, and a series of receiver nodes which are managed by a coordinator. By measuring Time Delay of Arrival (TDoA) of RF and ultrasonic signal from the transmitter, distance from target to receiver node is calculated and sent to the coordinator through CAN network, and all the information are gathered in PC to estimate 3D position of the target. Sine wave detector and dynamic threshold filter are applied to provide excellent accuracy of range measurement from TDoA result, and multilateration algorithms is realized to optimize coordinate determination accuracy. Specifically, Linear Least Square and Non-linear Least Square techniques are implemented to contrast their performances in target coordinate estimation. RF signal encoding/decoding time, time delay in CAN network and math calculating are carefully considered to ensure optimal system performance and get ready for field application. Experiments show that sine wave detector algorithm has greatly improved range measurement accuracy, with mean error at 2.2mm and maximum error at 6.7 mm, for distance below 5m. In addition, 3D position accuracy is greatly enhanced by multilateration methods, with mean error in position stay under 15mm, and 90% confident error values at 23mm for LLS, 20mm for NLS. Overall system update period has been verified in real system operation, with maximum rate at 25ms, and compared with other existing researches.


Introduction
In recent years, location-based applications have increased rapidly and become a great interest for many researchers as well as big technology companies.Location information is a key factor to enable a field of application involving in object guidance, path planning, or human assistance, etc.Therefore, localization is a hot trend in the age of information technology and robotics.Global Positioning System (or GPS), a global navigation satellite system that provides geolocation and time information to all GPS receivers anywhere on the Earth, is very popular currently.It is frequently used in daily activities and its receivers are embedded to every smart phone.For example, people are very easy to check where they are and navigate to a given point.Furthermore, GPS system can reach to the error of a few meters in outdoor environment.In case of poor satellite signal condition, A-GPS technology augments quality and precision of location by using cell tower data.That is the reason why GPS is the best choice for many outdoor localization applications.On the contrary, GPS shows bad performance in indoor environment due to multipath effects.Error of few meters is also not enough accurate for indoor applications where their workspace is relatively small and has many obstacles.Update rate is one more disadvantage of GPS with refresh frequency is only from 1 to 10Hz.As a result, many researchers are investigating to figure out high accurate, high data rate indoor localization system.In survey and review reports ( [1] to [6]), the authors overview many studies and their specification in recent decades.Many techniques and their highlight properties are summarized as well.The infrared signal technology was first introduced by Andy Harter, Andy Hooper and their colleagues at AT&T Lab of Cambridge University in the system called Active Badge [7].This system is room level localization and update period is about 15 seconds which is relatively slow.However, because of its simple and convenience, it was rolled out and widely used in a number of European and US universities.Ultrasound is the next solution that many researchers choose to implement.TDoA technique is normally used to measure distance from a transmitter to receivers or vice versa and then, target position is determined by using these distances and applying lateration methods.Active Bat of Andy Ward and his colleagues ( [8] and [9]), Cricket Indoor Localization of Priyantha and his team at MIT ( [36] and [37]) are good completed systems which pioneer this technique.Ultrasound-based positioning systems provide an accuracy up to centimeter range but require a stationary structure and line of sight (LoS) condition, it means user has to ensure that there is no obstacle between transmitter and receiver.For more flexible, using radio frequency (RF) is a solution.Non-LoS, wide range and easy to use are its advantages.In exchange, the system accuracy is only at room level or 1-3 meters.RSSI technique which is based on information about power of received signal is used to determine distance between target and access points.Especially, Ultrawide Band (UWB), a low power RF signal with wide spectrum of frequency bands, can be used to measure distance by TDoA or Time of Flight (ToF) technique.Moreover, using UWB even enables user to determine Angle of Arrival (AoA) as reviewed in [25].Ubisense 7000 sensor applied both of these techniques (TDoA and AoA).It supports operating range up to 160m and accuracy around 15cm in 3D with update rate can be configured from 0.1 to 134 Hz [38].Decawave and Bespoon are also use UWB technology and their comparison is shown as in [14].In case of combining with other visual purposes like map reconstruction, obstacle avoidance, etc., stereo vision is the best choice.This technique uses image processing algorithms and relies on feature points of captured images.However, it is not stable in dynamic environment where it has moving objects or light condition is not good.It is also hard and complex to use because stereo camera needs to be calibrated before putting in use.Moreover, stereo vision provides relative location information.It means user is required to set initial position and error is accumulated.In recent years, data integrating and fusion are preferred because single sensor signal has nearly reached their limitation.Additionally, this new approach combines two or more methods which diminish the imperfections of each other.For example, in short term, odometry can provide results more accurate than ultrasound system but its error is accumulated, which leads to high variation for a long time of operation.On the other hand, ultrasound-based positioning system is an absolute localization, and can be added to minimize the error generated by odometry.This is the main idea of [15] and [16].This combination introduces higher performance system by increasing update rate and enhancing measurement result.Theoretically, there are many choices to fuse positioning sources, especially with Ultrasound-based system such as IMU and portable Ultrasound Range Sensor [19], Laser Range Finder and Ultrasonic Beacon [20], or the others likes IMU and RFID [17], IMU and LIDAR sensor [21].Of course, data fusion has been carried out by applying Kalman filter and Particle filter in most of researches.
Back to the topic of ultrasound positioning system, TDoA uses two kinds of signal, usually RF and Ultrasonic wave, which have different speed in normal atmosphere.And key point of TDoA technique is detecting ultrasound arrival signal.Wave nature of ultrasound is mechanical oscillation, and transducer works as a converter from mechanical wave to electronic wave.The electronic signal is therefore a sine wave with a rising amplitude.Determining exactly the time when incoming signal arrives is certainly important and it affects system accurate seriously.The first method which people usually think about and implement is using threshold.Input signal is compared with a constant voltage to determine the time at which input value exceeds the threshold.At the same time, receiver is notified that ultrasound wave has arrived.The accuracy of this method defends on choosing a proper threshold.It is used in Active Bat and Cricket system as a simple and effective solution.But the error of distance measurement is high, about 2-3 cm, and leads to an increase in error of target coordinate determination as experimental results of our system in version 1 which is introduced in [18].Correlation method developed in [23] and [24] gives better results.Input signal is correlated with pre-configured signal which is a reference sine wave, then peak value indicates the moment at which ultrasound signal arrives.This method cuts distance measurement error down to sub-cm.
Another method was also applied is interpolation or interpolation -phase correction combination which are implemented in [22].It enables user to decrease not only their sampling frequency to under 40kHz but also the error of measurement to sub-cm range.
In this paper, we propose SNSH indoor localization system where sine wave detector method is applied to improve range measurement from TDoA result and Least Squares algorithms is realized to optimize error of coordinate determination.These two steps named measurement phase and processing phase.Similar to other previous researches, input signal is pre-processed by amplifier and filter circuit, then digitalized using ADC peripheral and stored.Generally, sine wave detector algorithm calculates interrelationship between input signal and pre-defined sine wave signal by integrating the multiplying result of these signals at the end of each period.Output of the detector allows us to find out when the incoming signal arrives.Therefore, this method improve accuracy of TDoA measurement in the first phase.
Simultaneously, Least Squares Estimation methods (LSE) are also implemented in the second phase.As stated earlier, distance measurement error in measurement phase causes error of target coordinate determination.Thus, LSE figures out object position by optimizing sum of squares residual between object and reference points.Linear Least Squares (LLS) and Nonlinear Least Squares (NLS) ( [26], [27], [28] and [29]) are realized and compared.Simulation and experiments have been carried out to verify these proposed methods.To allow field application and system management, CAN protocol is practiced building a network of reference nodes and coordinator for data exchange and system configuration.Target coordinate which is the final result of system is stored on a TCP/IP server and ready to be requested from any client through LAN network access.Improvement of two phases makes SNSH perform at higher efficiency.Variance of object positioning is about 1.5cm at 90% and update rate can reach to 40Hz.Experiments are executed in a testing space 2.5m x 2.5m x 2.5m.
The structure of paper is as follow.Section 2 overviews about hardware architecture, especially introduces about CAN and its advantages as well as why CAN is the best solution in this case.Sinewave detector and Least Squares problems are presented in section 3.In this section, MATLAB simulation is also introduced to support our proposed methods.To prove the feasibility and effectiveness of this system, section 4 shows experimental results which are established and run on real hardware.Statistic and comparison are also derived.Conclusions are discussed in section 5.

System Description
The proposed system SNSH composes of two parts: mobile part and stationary part.Mobile part is a transmitter which is attached on a moving target, whereas stationary part includes a CAN network in which a coordinator and receiver nodes communicate to each other.Coordinate of moving target is determined and stored in a PC, which acts as a server.Distances from transmitter to all receiver nodes are transferred from coordinator to PC via serial connection.System structure is illustrated as Figure 1.
This system not only reaches centimeter accuracy, but also has extendibility and is feasible to apply into reality.CAN network has many advantages which are perfectly suitable with our setup.Section 2.1 summarizes some specifications of this network and its connection.Section 2.2 outlined device's design and system operation.

Controller Area Network (CAN) and its advantages
CAN is serial communication, message-based protocol which is initiated by BOSCH for automotive application.It effectively supports distributed real-time control system because of many dominating characters.Since 1993, CAN bus has been standardized by International Organization for Standardization (ISO) in ISO 11898 ( [31] and [32]).CAN is one of five protocols which are chosen to use in On-Board Diagnostic (OBD-II), a vehicle diagnostic standard.These are the reasons why CAN is supported by many top semiconductor companies such as Texas Instrument, NXP, Microchip, etc. and become popular in variety of applications.Until now, BOSCH is still active in extending CAN standards and has just released CAN with Flexible Data-Rate (CAN FD) version 1.0 in 2012.
First, CAN replaces the complex wiring harness with a two-wire bus called CAN-H and CAN-L.The ISO 11898-2 Standard defines a single line of twisted-pair cable terminated at both ends with 120 Ohm resistors, which match the characteristic impedance of the line to prevent signal reflections ( [33] and [34]).Figure 2(a) represents a common CAN node/bus as described by ISO-11898 and that is also our system design, in which coordinator and receivers talk to each other.Additionally, signal on CAN bus is differential which allows CAN to acquire its robust noise immunity and fault tolerance.Differential signal reduces electromagnetic noise and allows high data rate over twistedpair cable.Specifically, CAN defines two logical states: recessive and dominant.The differential voltage of these states is specified as shown in Figure 2(b).With recessive state (logic '1'), differential voltage between CAN-H and CAN-L is less than minimum threshold, while with dominant state (logic '0'), this value is greater than minimum threshold.
Second, CAN is a carrier-sense, multiple-access protocol with collision detection and arbitration based on message priority (CSMA/CD+AMP).It means each node on a bus must wait for a prescribed period of inactivity before attempting to send a message.If there are two or more devices start transmitting their message at the same time, collision is resolved through bitwise arbitration mechanism.In this situation, dominant bit overdrives recessive bit on the bus to achieve nondestructive bitwise arbitration.Identifier field of a message represents its priority, and the device which has the higher priority identifier always wins the bus access and keeps on transmitting.A simple process is shown as in Figure 3. CSMA/CD+AMP is perfectly aligned with our system where many receivers connect to a network and many messages can be sent at the same time.Third, CAN is a message-based, multi-master protocol.It means there are many devices connect to the bus and may or may not be a master in a CAN network.Each device can talk to one another.
To achieve this goal, CAN message is broadcasted in network and devices can choose which one they want to accept by using a filter.Filter is performed on ID field of CAN message and one device can accept a range of ID from different kinds of other devices.This feature ensures the consistency of data in network.Moreover, it allows nodes can be added to or removed from the CAN network without requiring any change in the software or hardware of any other node, even when system is on operation (hot plug).CAN peripheral usually has two registers: mask and filter, and a filter can be created by combining both of them as an example in Table 1.

Mask Register 0b11111100000 0b11111111111
Filter Register 0b00100100000 0b00100100000 Finally, strong error-check and fault detection mechanism let CAN be one of the most popular protocols in industry.Theoretically, the total residual error probability of undetected corrupted messages is less than 4.7 x 10-11 as mention in [33].CAN protocol includes five methods of error checking: two at bit level and three at message level.Unless faulty message passes all of these methods, it is not accepted and error frame is generated to force transmitter resend last message until it is received correctly.At bit level, transmitter sends one by one and tracks the bits again by itself.There are two cases which make read value different to write value as follows.On the first hand, recessive bit is overwritten by a dominant bit in case device has lower priority than the other, in this case transmitter changes to listening mode and waits for its turn.On the second hand, Data bit is not corrected because of any problem from transmitter or physical connection; in this case an error is generated.Second technique at bit level is bit-stuffing rule which defines that transmitter automatically inserts a different bit into five consecutive bits of the same logic level and removes it at receiver side.At message level, as some industrial protocols, CAN uses 16 bit CRC checksum and ACK slot.Besides, frame check rule which defines some recessive bits at fixed position is also used.These bits are SOF (Start of Frame), EOF (End of Frame), ACK delimiter, and CRC delimiter.Their position is shown as in Figure 4.

Design and Operation
Transmitter device is equipped with an RF module for timing synchronization, a driver circuit and a transducer for generating ultrasound signal.IC SLN754410 is used to boost transmission power.Combined with IC 74HC14 Hex Inverting Schmitt Trigger, transducer's voltage is driven up to 24Vp-p, maximum value as recommended in ultrasonic transmitter 400ST120 specification.Main controller uses PWM peripheral to generate 40kHz signal as an input for this driver circuit.The schematic of ultrasound driver is shown as in Figure 5(a).The stack of a wireless protocol which is called SimpliciTI (also supported by TI) is open source and embedded on main controller.This is a simple RF protocol that supports IC CC110L sub-1GHz RF transceiver.To put system in operation, transmitter needs to be attached on target.At the beginning of each period, an RF signal is sent to make a trigger indication to Receivers, and a bunch of ultrasound is generated simultaneously.Emitting US pulse duration is 0.1ms (about 4 wave cycles as in Figure 5(b)) and managed by using a timer.Transmitter controls the sampling time of system by adjusting RF-US transmitting duration.
Currently, the maximum value of this period that ensures system functionality is at 20ms, or data update rate is 50Hz.Receivers in our proposed system act as reference points, receive and process US signal.Each receiver comprises a transducer 400SR120, an amplifier and a filter circuit, a CAN transceiver and a main processor.After received by the transducer, US beacon is first filtered by using a band pass filter comprised of Op-Amp and R, C components.This filter has frequency center around 40kHz, which allows the desired frequency range and removes unwanted noise in other frequencies.This circuit also amplifies US signal with a gain of 200.In the first version of our system, a threshold is applied to this pre-amplified signal using a comparator IC.The output of the comparator, in form of square wave, is used to indicate main processor that US signal has arrived.However, arrival angle and long travel distance substantially affect to the result.These factors can be diminished or even eliminated by using sine wave detection method which is proposed in section 3.1.Fundamentally, the preprocessed signal is converted to digital using ADC peripheral.Then, sine wave detection method is applied to this digital signal and find out exactly the time at which US signal arrived.All receivers connect to CAN network which is coordinated by Coordinator device and automatically send the TDoA information right after their processing procedure has done.Coordinator in our system is responsible for managing all receivers.It also operates as a gateway between PC and hardware system.Coordinator device includes an RF module CC110L and a CAN interface.This device receives RF trigger signal from transmitter device, generates a start message to all receivers and collects TDoA from all receivers via this CAN bus.Data exchange between them is based on high speed CAN protocol, up to 1Mbps.Then, this information is transferred to PC via serial interface.
Although main processor of coordinator device has enough performance to process, trilateration algorithm temporarily performs on PC software at the moment.This enables us to verify and change positioning methods easily.Moreover, a TCP/IP server is created to run in parallel with the positioning calculation.It publishes target coordinate result to the other network so that this information can be obtained from anywhere.It means that any client device connected to this network, including the target object, can use this information to carry on further positioning control techniques.There are two main advantages of using TCP/IP network for such application.First, it allows multiple clients to access the location data at the same time, and can be managed by the server.Second, TCP provides reliable, real-time and error-checked data stream, so it can be used in any control system that requires minimum chance of error.

Sine wave detection algorithm
Distance between the transmitter and the receiver is determined by measuring time of flight of ultrasonic wave.In order to obtain an accurate result, the moment when the wave arrives at the receiver must be captured precisely.Since ultrasound is mechanical wave, and the ultrasonic transducer at the receiver is used to convert mechanical vibration into electrical signal, the output signal requires a reasonable time to ramp up its amplitude.Therefore, when transmitting a 40kHz ultrasonic wave, the received signal has a form of sine wave with central frequency of 40kHz, and with a rising amplitude as shown earlier in Figure 6(b).
A common approach to detect ultrasonic wave is threshold mechanism.In this method, the amplitude of the receiver output is monitored continuously, and the moment it reaches a preconfigured DC voltage value is considered as the arrival of the wave.This method is easy to setup, it only requires a comparator circuit to detect a threshold voltage, without any processing power.When ultrasound arrives at the receiver, the sine wave amplitude will gradually increase, and when it exceeds the voltage threshold, the comparator will trigger the distance calculation process.
The above method was used in the first version of SNSH localization system, and it shows that the accuracy of this approach is determined by choosing a proper threshold value.A small threshold allows detecting the presence of ultrasonic wave faster, but it is also more susceptible to wrong pulse from the echoed wave and random noise from the circuit.On the other hand, a larger threshold value is more resilient to circuit noise and echoed wave, but it requires the wave to ramp up its amplitude for a few wave cycles before it can be reached.Therefore, the calculated distance with large threshold usually longer than the actual distance.As a result, this threshold detection approach only works well when the received wave is strong and rises up quickly; specifically, when the distance is short and the angle between the transmitter and receiver is narrow.
To better identify arrival ultrasonic wave at the receiver, the detector should be able to recognize the presence of wave pulse at which its amplitude is small.In this paper, a sine wave detection algorithm is proposed to address the drawbacks of threshold detection method.The theory of this new method is creating a local copy of waveform with similar characteristics of the wave that need to be recognized, then correlates it with the incoming wave.The correlation is proceeded by multiplying the incoming signal with the local copy sample by sample for a detection duration T, then integrating the calculated results.If the two signals are similar, multiplying them is basically square the incoming wave, and integrating the results for a time duration will generate a large number.On the other hand, if the incoming wave is different from the local waveform, the multiplying process will produce minus values, and integrating them results in a smaller value.
Let the incoming wave is formed as follow: With f0 is the incoming sinusoidal wave frequency, θ is the initial phase, and A is wave amplitude.This initial phase is an important factor because the received wave is not always in phase with the local replica.Then, let the local copy waveform with frequency f by Mathematically, the detector calculates the integral in a period T=1/f0 given by Solve the integral above, the output of the detector is determined As we can see, the integral result depends on the initial phase θ of the incoming wave.When θ = 90 0 , the detector output will be zero although the incoming wave and the local replica share the same frequency.To handle this circumstance, the incoming wave will be multiplied with a second local copy, which is 90 0 out of phase with the first copy.Specifically, the second local waveform and its integrating result will be Let T=1/f0 (a period of 40kHz sine wave signal) and substitute into equation (3.5) and (3.7), we have And therefore Besides, the average power of sine wave with amplitude A within a period T = 1/f0 can be calculated by In conclusion, the result of I 2 +Q 2 is independent of phase difference between incoming signal and local copy.Moreover, output of the sine wave detector will be basically proportional to the power of sine wave only when both of these signals have the same frequency.In other way, the difference in frequency makes magnitude of I 2 +Q 2 decrease.Thus, the detector operates as filter also to eliminate the effect of noise.

Implementation and simulation result
As mentioned above, sine wave detection algorithm can be considered as a new technique to detect the arrival of the incoming wave by measuring power of ultrasonic wave at the receiver.The fundamental procedure is first sampling the output signal of the receiver continuously at a constant sampling frequency, then doing sine wave detection algorithm at the end of each wave cycle.The output of the wave detector, which is proportional to wave energy, will be used to determine the existence of ultrasonic wave.Because the wave detector only responses to wave at 40kHz, its output is not influenced by random noise and more sensitive to ultrasonic wave at small amplitude.
A MATLAB simulation model has been designed to demonstrate the performance of the wave detector.In theory, the ultrasonic waveform is a continuous time signal, and the detector must do two integral calculations to achieve the result.However, this method is not applicable to digital processing, where wave is sampled at constant frequency and calculation is applied to an array of samples.Therefore, the MATLAB modelling will be constructed in discrete time domain, where sampling frequency plays an important role on the system.

Figure 7. MALAB model of sine wave detector in time domain
The modeling system consists of two components: a sample of ultrasonic wave, and a wave detector.First, the ultrasonic wave sample is obtained by ADC conversion at the receiver output.The digital buffer is then stored in the text file and imported to MATLAB as an array.Second, a wave detector is established by applying the detector theory in discrete time domain.Two local copies of 40kHz sine wave is created, specifically as (2 ) and (2 ), where f0 = 40kHz (frequency of ultrasonic signal).Next, a detector duration T is chosen as one wave cycle, which is T=1/f0.Then, the output of the detector will be calculated as described in the detector theory, and the integral function will be replaced by add function in discrete time domain.In conclusion, the simulation model demonstrates that the proposed wave detector is very effective.The output result is proportional to the square of sine wave amplitude and independent of initial phase, so it is easy to detect the presence of ultrasonic wave at very small amplitude, without affected by random noises.Moreover, this method does not require too much calculation and gives result at each wave cycle, whereas correlation algorithm which is applied in [23] and [24] have to recalculate the correlation coefficient of the wave sample sequence.Along with high performance operation of MCU, this detector is easily embedded to receiver board.

Least Square Problem
Classical approach of 3D positioning problem considers the intersection point of spheres whose centers are the locations of reference points Ri(xi, yi, zi) as target coordinate T(x, y, z).
Where di is an exact distance from target to reference point i.Solving n nonlinear equations provides us coordinate of the point of intersection.However, this procedure takes much computing time and resource because of the complexity of equations.Additionally, the equations are quadratic, they usually have two roots or more that need to be considered and verified.
Simultaneously linearizing equations converts this problem into easier one is to solve set of linear equations.This helps us to save number of calculations and simplify the procedure.Insert point Rj (xj, yj, zj) into equation (3.12) gives Expand and group again, leads to where i,j=1, 2, …, n is the number of beacons.di, dj, dij are distance between target and beacon i,j and distance between two beacon i and j, respectively.Rewritten in matrix form as follow Where Problem is converted into finding intersection of planes which is simpler than the original.But for n beacons, system of equations (3.15) has (n-1) equations in three unknown variables.Theoretically, only four beacons (n = 4) is needed to determine unique point of target in case we can measure distance correctly.
In fact, the distances between these beacons and target cannot be determined exactly, measurement noise causes incorrect information and requires us to have more than 4 receivers to increase accuracy.Range measurement is formed as equation (3.16) below where ri, εi are approximated distance between target and beacon i-th and measurement noise, respectively.Distance result follows normally distributed population (Gaussian) and measurement noise is considered as variance of this measurement.Location problem is therefore solved by using least square method which is discussed clearly below.
Least Squares algorithm defines an objective function or cost function which is the sum of squares residual, and optimizes system accuracy by determining x* to minimum this function.
Necessary condition for a local minimizer is that x* must be a stationary point of F or equivalent to solve the gradient equation, and sufficient condition is Hessian matrix For system of linear equations as in (3.15) above, this is linear least square problem in which function f is formed as x* is the solution of normal equations and obtained as follow Certainly, general method to solve least square problem is to find x* which satisfies both necessary and sufficient condition.However, it is very hard to solve this problem completely in general and it is not an effective way.Nonlinear Least Squares (NLS) presents another solution which solves the simpler of finding a local minimizer inside a certain region whose size is given by a small positive number δ. NLS problem in this case can be summarized as equation (3.21) follow Moreover, to develop on embedded system, it is necessary to establish a common approach which can be realized by programming language on PC.Accordingly, iterative methods for nonlinear optimization are carried out as the procedure below.From a given point x(0), the program calculates a series of x(1), x(2), …, x(k), x(k+1) which satisfy descending condition There are some different approaches can be taken: Gauss-Newton, Levenberg-Marquardt, Powell's Dog Leg or hybrid methods which combine modified of these methods.In this paper, we present Gauss-Newton method, and the best initial value x(0) is obtained from LLS method which has just been discussed above.This method based on implementation of first derivatives of function.It is therefore very basic and numerically efficient because the second derivatives which can be challenge to compute are not required.First, F(x) is chosen as the sum of squares of the errors on the distances as stated earlier and describes as equation (3.22)   Generally, with h small, Taylor expansion of every function around x Where J(x) is Jacobian matrix or gradient of F(x) which contains the first partial derivatives of (x) (3.23) and (3.24) lead to The Gauss-Newton step hgn is chosen to minimize F(x+h).Therefore, necessary condition and sufficient condition must be satisfied for F(x+h) Let A = J T J, A T = A then A is symmetric.As demonstration in [29], if J is full rank and linearly independent also, this leads to A is positive definite.It also shows that F(x+h) has a unique minimizer which can be found by solving Where, in this case, J T J and J T f are as follow Thus, Gauss-Newton iterative method is now derived as follow.Stop condition can be a threshold of error or maximum of number of steps.

Implementation and simulation results
This section presents MATLAB program to simulate the operation of two proposed methods.Output results are also used to compare with other researches as well as with one another.The realization on MATLAB shows that LLS and NLS is not complex and they can be fully implemented by any language like C, C++ or by C# in our project.Simulation program includes of two parts.
At first, N random points are generated so that they belong to workspace of system.Distances from these points to reference nodes which are pre-defined are also derived.External disturbance or measurement noise is presented by a random value and it will be added to range measurement results.Variance of noise shows the maximum of measurement error or the accuracy of distance measurement.These distance results after adding noise will be under trilateration process to retrieve target coordinate in the second step.Of course, these coordinates will be different to original ones.The accuracy of algorithms is determined by finding out the deviation.This procedure is summarized as below

Experimental Results
This section presents our proposed method experiments.Each approach is implemented in real hardware and software, then data is collected to evaluate their efficiency and performance as well as their practicability.

Sine wave detector
Modern micro-controller unit (MCU) offers high-performance computing solutions for embedded systems where reliability, high availability, and real-time responses are needed.It also supports connectivity protocols and other features at a low cost.It provides standard industrial protocol CAN version 2.0 A/B.It has Direct Memory Access (DMA), a great feature of microcontroller system which allows peripheral to transfer data directly to or from memory without being handled by the processor.Thus, it is perfectly suitable for dealing with large stream of data from ADC peripheral with minimum delay in between.It has higher efficiency than interrupt routine, which requires the processor to manage data at each interrupt event.Floating Point Unit (FPU) is a part of the system designed to handle fast calculation of float number and power-efficient algorithm of digital signal control applications such as sensor fusion, motor control and power management.These three factors work together to enable single chip solution that reduces module size, increases cost efficiency and enables system scalability.In our experiments, an ARM Cortex M4-based MCU STM32F4 is chosen as the main processor of receiver nodes.
According to the system configuration, the transmitter will first transmit a pulse of ultrasonic wave after sending an RF signal.Next, the coordinator will wait for an RF trigger signal to initiate distance measurement procedure in all receiver nodes.Each receiver node will receive a broadcast start message on CAN bus, which is sent from the coordinator, to start distance measurement process.Then, three stages will run in sequence to determine the initial wave cycle: ADC conversion, sine wave detection algorithm and dynamic threshold filter as shown in Figure 10.First, processor starts ADC conversion of US receiver output signal after triggered by a start message from CAN bus.The conversion will run continuously in a sampling duration T, with fixed sampling frequency at 1.4MSPS.The sampling duration is chosen so that the receiver can capture the incoming wave at the longest distance in the workspace.For example, supposed that the maximum distance is 5m, the sampling duration T is calculated as follows On the second stage, the discrete wave data are split into small buffers, each one stores the information of one ultrasonic wave cycle.These small buffers are input of the sine wave detection algorithm.After calculating, output of the detector is wave energy at 40 kHz for each wave cycle.Next, these energy results will be passed to a dynamic threshold filter on the third stage.
This digital filter is designed to eliminate the effect of echo wave and circuit noise.In theory, the first beam arrived at the receiver is wave that sent directly from the transmitter and has biggest amplitude, followed by a bunch of echo waves with smaller volume.Therefore, by spotting the wave cycle with maximum energy, the direct wave is pinpointed and separated from the echo waves.Also, as maximum energy is resolved by bubble sort of energy data at each trigger cycle, this value is dynamic and unaffected by measurement range as well as transmitting angle that decides the amplitude of the incoming wave.Next, the wave cycle with maximum energy value is chosen as a starting point for the second step.Energy difference between the current cycle and the previous one is calculated repeatedly, starting from the detected maximum cycle back to the start of the buffer.The loop stops when a minimum threshold of energy difference is passed for the first time, and the current cycle is chosen as the starting point of ultrasonic wave.Finally, the time of flight is obtained by a calibration process to give a precise relationship between the initial wave cycle and the distance between the transmitter and the receiver.
Ideally, measurement time includes US flight time of max range and math processing duration, which is about more than 15ms for max range of 5m.However, there are also has many disturbance factors added to this period such as delay time of encoding/decoding RF message, delay time of CAN sending and receiving packets, reflection effect of US wave at receiver nodes, etc.Therefore, to reach maximum performance of system, operation period is evaluated and dropped off by experiments.At our lab conditions, the system operates steadily with precision under 1cm at update of 20ms, and all data about measurement time will be available at server after transferring by CAN in about 22ms.
As previously discussed, measurement time composes of constant delay.Thus, the relationship between distance d and corresponding time different of arrival Δt is given as equation ( 4   Theoretically, sine wave detector method involves a possible error in time measurement of a period (of 40kHz ultrasound signal), it approximates 8.6 mm in distance.

Least Square algorithms
After measurement phase has determined distances to all reference nodes, coordinate of object is retrieved using Least Square methods.Hardware setup and implementation of the second phase is realized as follows.
The coordinates of reference nodes are calibrated and fixed at pre-configuration points as shown in Figure 1.In our experiments, five reference nodes are mounted with their coordinates listed in Table 3.
Additionally, to ensure distance measurement error under 1 cm, test points are chosen so that they satisfy condition as mentioned previously in section 4.1: angle of arrival under 45 degrees.Transmitter has to be settled inside the workspace as Figure 1.Finally, in our experiments, transmitter is static object, which maintains its position during measurement and data processing.Approximate distances from transmitter to reference nodes are available after finishing the first phase.In the second phase, Least Square Estimation methods are implemented on C# language and Windows platform using result from the first phase.After target coordinate is figured out by applying LLS and NLS methods, positioning result is recorded along with the approximate distances in text file.Short description of these two steps is presented as flowchart in Figure 13.
True positions and calculated points which are determined using Least Square methods are compared to illustrate the effectiveness of the algorithms.The evaluation is shown as follow in Figure 14 and Table 4.  Indoor localization application of Frequency Not mentioned 50 In brief, NLS certainly achieves higher accuracy than LLS.Cost function of NLS describes exactly the error which user needs to optimize.Meanwhile, LLS's cost function is figured out after linearization as Target point needs to be addressed in LLS method is (x-xj, y-yj, z-zj), and cost function includes more measurement numbers di, dj.Error therefore is amplified and higher than NLS.Vice versa, realization of LLS is much simpler than NLS and takes less math operators, especially, matrix multiplying and inversing.Cost function does not have to be calculated repeatedly in each step in the iteration as NLS.Fortunately, the performance and resource of modern embedded systems are constantly increasing so they can perform more sophisticated functions than in the past.Accordingly, there is not a big problem to implement NLS on a microcontroller like STM32F4 in our system.
As mentioned previously, distance measurement period is 22ms.After range estimation data is available at PC, LLS and NLS take about 3ms to resolve location problem and produce the coordinate of target.Experiments show that 25ms is minimum value for stable operation.Table 5 below is the comparison to other completed indoor localization system.

Conclusion
Various distance measurement techniques in ultrasonic-based localization system have been developed and introduced highly accurate results.In this paper, an innovative approach to detect the arrival of ultrasonic wave has been investigated using sine wave detector.Its implementation contributes another way to determine distance between two moving points using TDoA technique based on ultrasonic and RF signals.Compared with other mechanisms, range analysis using sine wave detector offers higher accuracy than conventional threshold method, and is as accurate as correlation technique, generally remains below 1 cm.Besides, it takes less time to perform a completed algorithm as well as doesn't repeat the calculation for each sample like correlation mechanism.Therefore, it can be embedded to main board and operates in real-time as a standalone unit.Accuracy and update rate are verified and mentioned clearly in real deployment.However, due to the effective beam angle constraint of ultrasonic transmitter, the proposed method only works well at maximum angle of arrival at 45 o .For this reason, position of target needs to be restricted in a workspace that satisfies this condition.
Additionally, real indoor localization system called SNSH has been carried out based on range analysis using multilateration algorithms.CAN network, which shows prominent characteristics as discussed in section 2, serves as communication protocol between coordinator and a number of reference nodes.Proposed hardware system demonstrates high reliability in data transfer, ease in installation, operation and maintenance process.Especially, system can be extended and upgraded up to (2 11 -1) reference nodes based on addressing of CAN protocol without change of hardware structure, and only requires minor steps of updating new nodes information in software.Two multilateration algorithms, LLS and NLS, are applied and experimented in SNSH localization system to resolve optimal 3D position from distance measurement result.In our final system deployment, average error in position stays under 15mm, with 90% confident error values at about 23mm for LLS and 20mm for NLS.A short comparison between SNSH system and previous indoor localization architectures concludes this paper in Table 4 and Table 5.
Our long-term goal is to build full standalone system with main board run on Linux, along with easy configurable software and devices because current localization algorithms run on PC and cannot adjust reference node coordinates seamlessly (only by hard-coding).Furthermore, CAN network can be utilized for other purposes.Its bandwidth is sufficient to handle other data streams like sensor acquisition or remote control, etc.On the other hand, system period or sampling time can be further cut down since modern MCU has been increased processing speed and can perform complex digital signal processing in short time.Most of highly accurate localization systems are under evaluation for verifying algorithms and only designed for laboratory experiments.In this paper, we presented a completed solution for indoor positioning problem including two phases measurement and localization, and released a fully functional evaluation system for this purpose.

Figure 4 .
Figure 4. CAN standard frame

Figure 5 .
Figure 5. (a) Schematic of US driver (b) US transmitter signal

Figure 6 .
(a) Amplifier and Filter circuit (b) US receiver signal after pre-processed

Figure 8 .
Figure 8. MATLAB simulation result with real wave signal at sampling rate 1.4MSPS.

Figure 9 .
Figure 9. MATLAB simulation, the comparison of LLS and NLS algorithm

Figure 10 .
Figure 10.Block diagram of process sequence in receiver node .1) below .d v t c    (4.1)where: v (slope) is ultrasound speed (theoretically, v≈ 340 m/s) and c (offset) is a constant.

Figure 11 .
Figure 11.Two steps in Dynamic Threshold Filter

Figure 13 .Figure 14 .
Figure 13.Operation flow of current demo system

Table 2 .
Experiment results of distance measurement and comparison (range up to 6 meters)

Table 3 .
Coordinate of reference nodes (in centimeter)

Table 4 .
Evaluation of LLS and NLS in our experiments and comparison to other systems CDFPreprints (www.

Table 5 .
Comparison of update rate of proposed system and others