Algebraic expressions reduction

[jeffs]

Hello,
this is my first post here, I read the rules for new posters, non-native speaker, so I'm not completely sure whether I'm not doing something completely wrong

Background:
I'm solving practical problem of navigating a robot using time-of-flight of ultrasound waves. It is kind of similar to GPS, but much more crude and using slower propagating waves, allowing much simpler electronics, perhaps there are other advantages to be discovered. Using intuition I came to conclusion that I need three ultrasound transmitters for one receiver. The transmitters are perfectly synchronized to each other, but receiver has no precise timebase, though it can measure distance between ultrasound beeps with reasonable accuracy. I'm good at electronics and practical side of things, but I realized how badly I failed with math.
Plan goes like this: Three transmitters A, B and C are located in known positions. Transmitter A makes a short beep, silences itself and waits. After a known time, transmitter B makes a beep, waits, then C makes a beep and cycle can repeat. Receiver R doesn't know the exact time of when A beeped, but can measure time difference between A and expected beep B, as well as difference between A and C. I assume those two time delays should be enough to calculate two dimensional position of receiver R relative to A, B and C.

Reduction to trigonometry and later algebraic expressions:
For sake of simplicity, lets forget there are time differences between beeps and just assume all three transmitters do a single beep at the same time and receiver can somehow distinguish between them. I drew a crude picture, please see the attachment. If the picture is unreadable, let me know, I can redraw it. It also covers the part of calculation I can do, but got stuck later.

There are Cartesian coordinates with four points: A, B, C and R. A is always at (0,0), B is always at (By,0) and c is always at (0,Cx). By and Cx are known constants. R unknown and I need to calculate its position (coordinates Rx and Ry).
Equations 1A, 1B and 1C are my take on calculating the lengths from receiver to point A, B and C respectively.
Sound wave from A takes time T0 to arrive to receiver R. T0 is unknown to receiver. Sound wave from B takes (T0+D1) that is, it hits receiver D1 difference later than T0 (ie sound from A). Similarly, sound wave from C hits receiver at time T0+D2. Equations 2A-2C capture this, plus it is multiplied by S (speed of sound, it is known constant), so that lengths AR, BR and CR are expressed as time of flight of respective sound wave.
From equations 1 and 2 I combined equations 3 to get set of three equations with three unknown variables - I thought this should be simple now.
I entered the equation 3A into equations 3B and 3C and got 4A and 4B. Since T0 is unknown, I expressed it from 3A as equation 5 and made substitution in 4A and 4B, got results 6A and 6B. Both equations looks somehow reasonable and "symmetric", but I don't know how to proceed from this point on.

Expected result:
My ultimate goal is to calculate Rx and Ry, from known values S, By, Cx, D1 and D2. T0 is unknown, but I hoped it will dissapear at the end (it should be irrelevant since I have three transmitters). I'd say that equations 1 and 2 are OK, but perhaps the rest could be done differently.

Thank you for your help.

 

[Cubist]

I think your equations 1A to 1C are good, however 2A to 2C are not quite right. Perhaps consider the following instead...

If transmitter A sends its beep at absolute time t=T_A and the robot receives the beep at t=T₀ then

|AR| = ( T₀ - T_A )*S

If transmitter B sends its beep after a delay of T_d and the robot receives it at t=T₁ then

|BR| = ( T₁ - (T_A+T_d) )*S

If transmitter C sends its beep after another delay of T_d and the robot receives it at t=T₂ then

|CR| = ( T₂ - (T_A+2*T_d) )*S

I think if you can also say that R_y > 0, then there should be enough information to determine all the unknowns (obviously T_d is known).

 

[jeffs]

Hello,
thank you for your response and pointing out the missing delay between beeps. Yes, you are correct, the Td is missing here. I intentionally omitted this delay - the receiver will see the delay but it should be easy enough to subtract this delay and start computing as if all three transmitters beeped at the same time. I did to decrease the amount of variables in my formulas, but still, with this simplified approach I just can't get my head around it. I'm stuck at step 6 and can't get any further.

 

[Cubist]

1A) [math] |AR|^2 = R_x^2 + R_y^2 [/math]
1B) [math] |BR|^2 = R_x^2 + (B_y - R_y)^2[/math]
1C) [math] |CR|^2 = R_y^2 + (C_x - R_x)^2[/math]
2A) [math] |AR| = ( T_0 - T_A )S[/math]
2B) [math] |BR| = ( T_1 - (T_A+T_d) )S[/math]
2C) [math] |CR| = ( T_2 - (T_A+2*T_d) )S[/math]
1A&2A give 3A) [math] ( T_0 - T_A ) S = \sqrt{R_x^2 + R_y^2} [/math]
1B&2B give 3B) [math] ( T_1 - (T_A+T_d) ) S = \sqrt{R_x^2 + (B_y - R_y)^2} [/math]
1C&2C give 3C) [math] ( T_2 - (T_A+2T_d) ) S = \sqrt{R_y^2 + (C_x - R_x)^2}[/math]
--

3B-3A) [math] (T_1 - T_0 - T_d) S = \sqrt{R_x^2 + (B_y - R_y)^2} - \sqrt{R_x^2 + R_y^2} [/math]
3C-3A) [math] (T_2 - T_0 - 2*T_d) S = \sqrt{R_y^2 + (C_x - R_x)^2} - \sqrt{R_x^2 + R_y^2} [/math]
At this point we've eliminated \(T_A\) from the above two equations. The variables on the LHS are all known, so we can call them c1 and c2. Then we're effectively left with:-

[math] c_1 = \sqrt{R_x^2 + (B_y - R_y)^2} - \sqrt{R_x^2 + R_y^2}[/math][math] c_2 = \sqrt{R_y^2 + (C_x - R_x)^2} - \sqrt{R_x^2 + R_y^2}[/math]
I can't currently see how to simplify these down to \(R_x=\) and \(R_y=\). A numerical solver should be able to find a unique solution for \(R_x>0\) and \(R_y>0\). But I will think some more about this!

 

[jeffs]

Thank you very much. Your approach makes much more sense than my clunky attempts and the resulting equations look perfectly reasonable.

Analytic solution would be ideal, since I have to solve the equations in relatively small computer on board of the receiver. If it can't be found, numerical solving is also possible, but I need to code it on my own. It's probably not a rocket science, but it probably needs a bit of experience to determine appropriate solving method.

 

[jeffs]

Just a follow-up.
i played with the equations the Cubist provided and found quick algorithm to solve it numerically - https://en.wikipedia.org/wiki/Hill_climbing I implemented it quickly, tested with data from simulation and it seems to do the job. Three or four iterations are enough to find Rx and Ry with good precision, allowing real-time operation and still leaving plenty of processor time.

Thanks again.

 

[Cubist]

I'm pretty sure an analytic solution isn't possible. It's beyond my abilities anyway! So I too had a play with a numerical solution. I used a binary search method, alternating between searching in x and then in y. So this probably isn't the quickest method - but it was very simple to code and it gets good results.

import sys
import math

def fn_y(Rx, By, c1, y):
    return math.sqrt(Rx**2 + (By - y)**2) - math.sqrt(Rx**2 + y**2) - c1

def fn_x(Ry, Cx, c2, x):
    return math.sqrt(Ry**2 + (Cx - x)**2) - math.sqrt(x**2 + Ry**2) - c2
   
def binarySearch(fn, param1, param2, param3, bot, top, acc):
    bv=fn(param1, param2, param3, bot)>=0
    tv=fn(param1, param2, param3, top)>=0
    if tv==bv: sys.exit(0) # Should do something more sensible here if this should happen
    while top-bot>acc:
        mid=(top+bot)*0.5
        mv=fn(param1, param2, param3, mid)>=0
        if mv==bv:
            bot=mid
        else:
            top=mid
    return (top+bot)*0.5

# Locations of B and C
Cx=6
By=4

def findXY(c1,c2):
    Rx=0
    Ry=0
    accuracy=0.0001
    margin=0.5
    for i in range(5): # This seems like enough repeats to obtain good accuracy
        Ry=binarySearch(fn_y,Rx,By,c1, -margin, By+margin, accuracy)
        Rx=binarySearch(fn_x,Ry,Cx,c2, -margin, Cx+margin, accuracy)
    return(Rx,Ry)

#################################################
# Test with the robot on grid locations
for x in range(Cx):
    for y in range(By):
        S=2
        TD=30

        tx=x + 0.5
        ty=y + 0.5
        # Calculate theoretical times thet the robot would receive the signals...
        t0=math.sqrt(tx**2 + ty**2)/S
        t1=math.sqrt(tx**2 + (By-ty)**2)/S + TD
        t2=math.sqrt((Cx-tx)**2 + ty**2)/S + 2*TD

        # Now, calculate the position using the times...
        c1=(t1-t0-TD)*S
        c2=(t2-t0-2*TD)*S
        (Rx,Ry) = findXY(c1,c2)
        print("%0.4f  %0.4f" % (Rx, Ry))
    print()

 

[jeffs]

Thank you so much for this complete package, your help was really important. For now I'm considering this topic to be solved.
The simulated results look good, in a few weeks I should have some practical results, I'll share it here.