Ultrasonic sensor, Trilateration

• Ultrasonic 

- The sound waves with frequencies above 20kHz, which are beyond human hearing.
- Ultrasonic sensors use high-frequency sound waves to detect objects and measure distances.
- It is a time-of-flight sensor that operates by calculating the time when the transmitted wave to return.

- Sound waves used by ultrasonic sensors cannot be heard by humans because they must be transmitted above high frequency (100 dB) to accurately receive reflected sound waves.

- Sound waves have a large opening angle, it uses the trilateral measurement of overlapping signals.

- Sound waves can accurately locate an object using the same principle as satellite-based positioning.

 

• Trilateration 

On the diagram above, each circle represents all the possible locations of a mobile phone at a given distance (radius) of a cell tower. The aim of a trilateration algorithm is to calculate the (x,y) coordinates of the intersection point of the three circles. Each circle is defined by the coordinates of its center e.g. (x1,y1) and its radius e.g. r1.

 

Step 1
The three equations for the three circles are as follows:

Step 2:
We can expand out the squares in each of these three equations:

Step 3:
Now let’s subtract the second equation from the first:

Likewise, we can now subtract the third equation from the second:

Step 4:
Let’s rewrite these two equations using A, B, C, D, E, F values. This would result in the following system of 2 equations:

Step 5:
The solution of this system is:

import numpy as np

class AP:
    def __init__(self, x, y, distance):
        self.x = x
        self.y = y
        self.distance = distance

class Trilateration:
    def __init__(self, AP1, AP2, AP3):
        self.AP1 = AP1
        self.AP2 = AP2
        self.AP3 = AP3
    
    def calcUserLocation(self):
        A = 2 * (self.AP2.x - self.AP1.x)
        B = 2 * (self.AP2.y - self.AP1.y)
        C = self.AP1.distance**2 - self.AP2.distance**2 - self.AP1.x**2 + self.AP2.x**2 - self.AP1.y**2 + self.AP2.y**2
        D = 2 * (self.AP3.x - self.AP2.x)
        E = 2 * (self.AP3.y - self.AP2.y)
        F = self.AP2.distance**2 - self.AP3.distance**2 - self.AP2.x**2 + self.AP3.x**2 - self.AP2.y**2 + self.AP3.y**2
        
        user_x = ( (F * B) - (E * C) ) / ( (B * D) - (E * A))
        user_y = ( (F * A) - (D * C) ) / ( (A * E) - (D * B))
        return user_x, user_y
    
if __name__ == "__main__":
    ap1 = AP(4, 4, np.sqrt(32))
    ap2 = AP(12, 4, np.sqrt(32))
    ap3 = AP(8, 12, 4)

    tril = Trilateration(ap1, ap2, ap3)
    x, y = tril.calcUserLocation()
    print(x)
    print(y)
    
>>>
8.0
8.0

 

https://ahang.tistory.com/26

https://www.101computing.net/cell-phone-trilateration-algorithm/