True-range multilateration

This article needs additional citations for verification. Please help improve this article by adding citations to reliable sources. Unsourced material may be challenged and removed. Find sources: "True-range multilateration" – news · newspapers · books · scholar · JSTOR (June 2017) (Learn how and when to remove this message)

True-range multilateration (also termed range–range multilateration and spherical multilateration) is a method to determine the location of a movable vehicle or stationary point in space using multiple ranges (distances) between the vehicle/point and multiple spatially-separated known locations (often termed "stations"). ^[1][2] Energy waves may be involved in determining range, but are not required.

True-range multilateration is both a mathematical topic and an applied technique used in several fields. A practical application involving a fixed location occurs in surveying.^[3][4] Applications involving vehicle location are termed navigation when on-board persons/equipment are informed of its location, and are termed surveillance when off-vehicle entities are informed of the vehicle's location.

Two slant ranges from two known locations can be used to locate a third point in a two-dimensional Cartesian space (plane), which is a frequently applied technique (e.g., in surveying). Similarly, two spherical ranges can be used to locate a point on a sphere, which is a fundamental concept of the ancient discipline of celestial navigation — termed the altitude intercept problem. Moreover, if more than the minimum number of ranges are available, it is good practice to utilize those as well. This article addresses the general issue of position determination using multiple ranges.

In two-dimensional geometry, it is known that if a point lies on two circles, then the circle centers and the two radii provide sufficient information to narrow the possible locations down to two – one of which is the desired solution and the other is an ambiguous solution. Additional information often narrow the possibilities down to a unique location. In three-dimensional geometry, when it is known that a point lies on the surfaces of three spheres, then the centers of the three spheres along with their radii also provide sufficient information to narrow the possible locations down to no more than two (unless the centers lie on a straight line).

True-range multilateration can be contrasted to the more frequently encountered pseudo-range multilateration, which employs range differences to locate a (typically, movable) point. Pseudo range multilateration is almost always implemented by measuring times-of-arrival (TOAs) of energy waves. True-range multilateration can also be contrasted to triangulation, which involves the measurement of angles.

There is no accepted or widely-used general term for what is termed true-range multilateration here . That name is selected because it: (a) is an accurate description and partially familiar terminology (multilateration is often used in this context); (b) avoids specifying the number of ranges involved (as does, e.g., range–range; (c) avoids implying an application (as do, e.g., DME/DME navigation or trilateration) and (d) and avoids confusion with the more common pseudo-range multilateration.

For similar ranges and measurement errors, a navigation and surveillance system based on true-range multilateration provide service to a significantly larger 2-D area or 3-D volume than systems based on pseudo-range multilateration. However, it is often more difficult or costly to measure true-ranges than it is to measure pseudo ranges. For distances up to a few miles and fixed locations, true-range can be measured manually. This has been done in surveying for several thousand years  – e.g., using ropes and chains.

For longer distances or moving vehicles, a radio/radar system is generally needed. This technology was first developed circa 1940 in conjunction with radar. Since then, three methods have been employed:

• Two-way range measurement, one party active  – This is the method used by traditional radars (sometimes termed primary radars) to determine the range of a non-cooperative target, and now used by laser rangefinders. Its major limitations are that: (a) the target does not identify itself, and in a multiple target situation, mis-assignment of a return can occur; (b) the return signal is attenuated (relative to the transmitted signal) by the fourth power of the vehicle-station range (thus, for distances of tens of miles or more, stations generally require high-power transmitters or large/sensitive antennas); and (c) many systems utilize line-of-sight propagation, which limits their ranges to less than 20 miles when both parties are at similar heights above sea level.
• Two-way range measurement, both parties active  – This method was reportedly first used for navigation by the Y-Gerät aircraft guidance system fielded in 1941 by the Luftwaffe. It is now used globally in air traffic control – e.g., secondary radar surveillance and DME/DME navigation. It requires that both parties have both transmitters and receivers, and may require that interference issues be addressed.
• One-way range measurement  – The time of flight (TOF) of electromagnetic energy between multiple stations and the vehicle is measured based on transmission by one party and reception by the other. This is the most recently developed method, and was enabled by the development of atomic clocks; it requires that the vehicle (user) and stations having synchronized clocks. It has been successfully demonstrated (experimentally) with Loran-C and GPS.^[2][5]

This section needs expansion. You can help by adding missing information. (June 2017)

True-range multilateration algorithms may be partitioned based on

• problem space dimension (generally, two or three),
• problem space geometry (generally, Cartesian or spherical) and
• presence of redundant measurements (more than the problem space dimension).

Any pseudo-range multilateration algorithm can be specialized for use with true-range multilateration.

An analytic solution has likely been known for over 1,000 years, and is given in several texts.^[6] Moreover, one can easily adapt algorithms for a three dimensional Cartesian space.

The simplest algorithm employs analytic geometry and a station-based coordinate frame. Thus, consider the circle centers (or stations) C1 and C2 in Fig. 1 which have known coordinates (e.g., have already been surveyed) and thus whose separation U {\displaystyle U} is known. The figure 'page' contains C1 and C2. If a third 'point of interest' P (e.g., a vehicle or another point to be surveyed) is at unknown point ( x , y ) {\displaystyle (x,y)} , then Pythagoras's theorem yields

r 1 2 = x 2 + y 2 r 2 2 = ( U − x ) 2 + y 2 {\displaystyle {\begin{aligned}r_{1}^{2}&=x^{2}+y^{2}\\[4pt]r_{2}^{2}&=(U-x)^{2}+y^{2}\end{aligned}}}

Thus,

Note that y {\displaystyle y} has two values (i.e., solution is ambiguous); this is usually not a problem.

While there are many enhancements, Equation 1 is the most fundamental true-range multilateration relationship. Aircraft DME/DME navigation and the trilateration method of surveying are examples of its application. During World War II Oboe and during the Korean War SHORAN used the same principle to guide aircraft based on measured ranges to two ground stations. SHORAN was later used for off-shore oil exploration and for aerial surveying. The Australian Aerodist aerial survey system utilized 2-D Cartesian true-range multilateration.^[7] This 2-D scenario is sufficiently important that the term trilateration is often applied to all applications involving a known baseline and two range measurements.

The baseline containing the centers of the circles is a line of symmetry. The correct and ambiguous solutions are perpendicular to and equally distant from (on opposite sides of) the baseline. Usually, the ambiguous solution is easily identified. For example, if P is a vehicle, any motion toward or away from the baseline will be opposite that of the ambiguous solution; thus, a crude measurement of vehicle heading is sufficient. A second example: surveyors are well aware of which side of the baseline that P lies. A third example: in applications where P is an aircraft and C1 and C2 are on the ground, the ambiguous solution is usually below ground.

If needed, the interior angles of triangle C1-C2-P can be found using the trigonometric law of cosines. Also, if needed, the coordinates of P can be expressed in a second, better-known coordinate system—e.g., the Universal Transverse Mercator (UTM) system—provided the coordinates of C1 and C2 are known in that second system. Both are often done in surveying when the trilateration method is employed.^[8] Once the coordinates of P are established, lines C1-P and C2-P can be used as new baselines, and additional points surveyed. Thus, large areas or distances can be surveyed based on multiple, smaller triangles—termed a traverse.

An implied assumption for the above equation to be true is that r 1 {\displaystyle r_{1}} and r 2 {\displaystyle r_{2}} relate to the same position of P. When P is a vehicle, then typically r 1 {\displaystyle r_{1}} and r 2 {\displaystyle r_{2}} must be measured within a synchronization tolerance that depends on the vehicle speed and the allowable vehicle position error. Alternatively, vehicle motion between range measurements may be accounted for, often by dead reckoning.

A trigonometric solution is also possible (side-side-side case). Also, a solution employing graphics is possible. A graphical solution is sometimes employed during real-time navigation, as an overlay on a map.

There are multiple algorithms that solve the 3-D Cartesian true-range multilateration problem directly (i.e., in closed-form) – e.g., Fang.^[9] Moreover, one can adopt closed-form algorithms developed for pseudo range multilateration.^[10][6] Bancroft's algorithm^[11] (adapted) employs vectors, which is an advantage in some situations.

The simplest algorithm corresponds to the sphere centers in Fig. 2. The figure 'page' is the plane containing C1, C2 and C3. If P is a 'point of interest' (e.g., vehicle) at ( x , y , z ) {\displaystyle (x,y,z)} , then Pythagoras's theorem yields the slant ranges between P and the sphere centers:

r 1 2 = x 2 + y 2 + z 2 r 2 2 = ( x − U ) 2 + y 2 + z 2 r 3 2 = ( x − V x ) 2 + ( y − V y ) 2 + z 2 {\displaystyle {\begin{aligned}r_{1}^{2}&=x^{2}+y^{2}+z^{2}\\[4pt]r_{2}^{2}&=(x-U)^{2}+y^{2}+z^{2}\\[4pt]r_{3}^{2}&=(x-V_{x})^{2}+(y-V_{y})^{2}+z^{2}\end{aligned}}}

Thus, the coordinates of P are:

The plane containing the sphere centers is a plane of symmetry. The correct and ambiguous solutions are perpendicular to it and equally distant from it, on opposite sides.

Many applications of 3-D true-range multilateration involve short ranges—e.g., precision manufacturing.^[12] Integrating range measurement from three or more radars (e.g., FAA's ERAM) is a 3-D aircraft surveillance application. 3-D true-range multilateration has been used on an experimental basis with GPS satellites for aircraft navigation.^[5] The requirement that an aircraft be equipped with an atomic clock precludes its general use. However, GPS receiver clock aiding is an area of active research, including aiding over a network. Thus, conclusions may change.^[13] 3-D true-range multilateration was evaluated by the International Civil Aviation Organization as an aircraft landing system, but another technique was found to be more efficient.^[14] Accurately measuring the altitude of aircraft during approach and landing requires many ground stations along the flight path.

The general multilateration problem consists of computing the unknown position p ^ = [ x ^ , y ^ , z ^ ] T {\textstyle {\boldsymbol {\hat {p}}}=\left[{\hat {x}},{\hat {y}},{\hat {z}}\right]^{\text{T}}} of some target node in 3D space. (Though, the approach described here works also in higher dimensions.) For that, it is assumed that there are N {\textstyle N} anchor nodes. The position p i = [ x i , y i , z i ] T {\textstyle {\boldsymbol {p}}_{i}=\left[x_{i},y_{i},z_{i}\right]^{\text{T}}} (for i = 1 … N {\textstyle i=1\ldots N} ) of each anchor node must be known. Moreover, it is assumed that the distances d i {\textstyle d_{i}} between each anchor node and the target node is known.

The distance d i {\textstyle d_{i}} between the i {\textstyle i} -th anchor node and the target node is given as follows. d i = ‖ p i − p ^ ‖ 2 = ( x i − x ^ ) 2 + ( y i − y ^ ) 2 + ( z i − z ^ ) 2 {\displaystyle d_{i}=\left\|{\boldsymbol {p}}_{i}-{\boldsymbol {\hat {p}}}\right\|_{2}={\sqrt {(x_{i}-{\hat {x}})^{2}+(y_{i}-{\hat {y}})^{2}+(z_{i}-{\hat {z}})^{2}}}} Below, two different algorithms are shown to compute the target node's position p ^ {\textstyle {\boldsymbol {\hat {p}}}} . The first algorithm computes the target position by only using the anchor node positions p i {\textstyle {\boldsymbol {p}}_{i}} and the distances d i {\textstyle d_{i}} between the target node and the anchor nodes. The second algorithm assumes that a rough estimate of p ^ {\textstyle {\boldsymbol {\hat {p}}}} is given. This algorithm then iteratively refines this estimate.

This algorithm computes the target node's position p ^ {\textstyle {\boldsymbol {\hat {p}}}} from the anchor node positions p i {\textstyle {\boldsymbol {p}}_{i}} and the (squared) distances d i {\textstyle d_{i}} . d i 2 = ‖ p i − p ^ ‖ 2 2 = ‖ p i ‖ 2 2 + ‖ p ^ ‖ 2 2 − 2 p i T p ^ {\displaystyle d_{i}^{2}=\left\|{\boldsymbol {p}}_{i}-{\boldsymbol {\hat {p}}}\right\|_{2}^{2}=\left\|{\boldsymbol {p}}_{i}\right\|_{2}^{2}+\left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}-2{\boldsymbol {p}}_{i}^{\text{T}}{\boldsymbol {\hat {p}}}} A somewhat unpleasant thing in the equation above is the term ‖ p ^ ‖ 2 2 {\textstyle \left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}} . To get rid of this term, one may subtract the mean from both sides of the equation d i 2 = ‖ p i ‖ 2 2 + ‖ p ^ ‖ 2 2 − 2 p i T p ^ {\textstyle d_{i}^{2}=\left\|{\boldsymbol {p}}_{i}\right\|_{2}^{2}+\left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}-2{\boldsymbol {p}}_{i}^{\text{T}}{\boldsymbol {\hat {p}}}} . d i 2 − 1 N ∑ k = 1 N d k 2 = ‖ p i ‖ 2 2 + ‖ p ^ ‖ 2 2 − 2 p i T p ^ − 1 N ∑ k = 1 N ( ‖ p k ‖ 2 2 + ‖ p ^ ‖ 2 2 − 2 p k T p ^ ) {\displaystyle d_{i}^{2}-{\frac {1}{N}}\sum _{k=1}^{N}d_{k}^{2}=\left\|{\boldsymbol {p}}_{i}\right\|_{2}^{2}+\left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}-2{\boldsymbol {p}}_{i}^{\text{T}}{\boldsymbol {\hat {p}}}-{\frac {1}{N}}\sum _{k=1}^{N}\left(\left\|{\boldsymbol {p}}_{k}\right\|_{2}^{2}+\left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}-2{\boldsymbol {p}}_{k}^{\text{T}}{\boldsymbol {\hat {p}}}\right)} The key is, that ‖ p ^ ‖ 2 2 {\textstyle \left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}} does not depend on k {\textstyle k} in the sum on the right-hand side. And therefore 1 N ∑ k = 1 N ‖ p ^ ‖ 2 2 = ‖ p ^ ‖ 2 2 {\displaystyle {\frac {1}{N}}\sum _{k=1}^{N}\left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}=\left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}} . Hence, ‖ p ^ ‖ 2 2 {\textstyle \left\|{\boldsymbol {\hat {p}}}\right\|_{2}^{2}} is removed from the right-hand side. d i 2 − 1 N ∑ k = 1 N d k 2 = ‖ p i ‖ 2 2 − 2 p i T p ^ − 1 N ∑ k = 1 N ‖ p k ‖ 2 2 + 1 N ∑ k = 1 N 2 p k T p ^ {\displaystyle d_{i}^{2}-{\frac {1}{N}}\sum _{k=1}^{N}d_{k}^{2}=\left\|{\boldsymbol {p}}_{i}\right\|_{2}^{2}-2{\boldsymbol {p}}_{i}^{\text{T}}{\boldsymbol {\hat {p}}}-{\frac {1}{N}}\sum _{k=1}^{N}\left\|{\boldsymbol {p}}_{k}\right\|_{2}^{2}+{\frac {1}{N}}\sum _{k=1}^{N}2{\boldsymbol {p}}_{k}^{\text{T}}{\boldsymbol {\hat {p}}}} This can be rearranged as follows. d i 2 − 1 N ∑ k = 1 N d k 2 − ‖ p i ‖ 2 2 + 1 N ∑ k = 1 N ‖ p k ‖ 2 2 ⏟ b i = 2 ⋅ ( 1 N ∑ k = 1 N p k − p i ) ⏟ a i T p ^ {\displaystyle \underbrace {d_{i}^{2}-{\frac {1}{N}}\sum _{k=1}^{N}d_{k}^{2}-\left\|{\boldsymbol {p}}_{i}\right\|_{2}^{2}+{\frac {1}{N}}\sum _{k=1}^{N}\left\|{\boldsymbol {p}}_{k}\right\|_{2}^{2}} _{b_{i}}={\underbrace {2\cdot \left({\frac {1}{N}}\sum _{k=1}^{N}{\boldsymbol {p}}_{k}-{\boldsymbol {p}}_{i}\right)} _{{\boldsymbol {a}}_{i}}}^{\text{T}}{\boldsymbol {\hat {p}}}} Using the abbreviations defined above, the equation for the i {\textstyle i} -th anchor node becomes b i = 2 a i T p ^ {\displaystyle b_{i}=2{\boldsymbol {a}}_{i}^{\text{T}}{\boldsymbol {\hat {p}}}} . By incorporating all anchor nodes, the multilateration problem can be written as single equation with the vector b = [ b 1 , … , b N ] T {\displaystyle {\boldsymbol {b}}=\left[b_{1},\dots ,b_{N}\right]^{\mathrm {T} }} and the matrix A = [ a 1 , … , a N ] T {\displaystyle {\boldsymbol {A}}=\left[{\boldsymbol {a}}_{1},\dots ,{\boldsymbol {a}}_{N}\right]^{\text{T}}} . A p ^ = b {\displaystyle {\boldsymbol {A}}{\boldsymbol {\hat {p}}}={\boldsymbol {b}}} The final task is to solve this equation for position p ^ {\textstyle {\boldsymbol {\hat {p}}}} of the target node. A usual choice is the least squares solution: p ^ = ( A T A ) -1 A T b {\displaystyle {\boldsymbol {\hat {p}}}=\left({\boldsymbol {A}}^{\text{T}}{\boldsymbol {A}}\right)^{\text{-1}}{\boldsymbol {A}}^{\text{T}}{\boldsymbol {b}}}

This algorithm can be used if a initial rough estimate of the target node's position p ^ {\textstyle {\boldsymbol {\hat {p}}}} is available. This initial estimate is used to iteratively compute a more accurate estimate of p ^ {\textstyle {\boldsymbol {\hat {p}}}} . For this, a Gauss–Newton algorithm is used with the following update equation. p ^ ( k + 1 ) = p ^ ( k ) − ( J ( k ) T J ( k ) ) -1 J ( k ) T f ( k ) {\displaystyle {\boldsymbol {\hat {p}}}^{(k+1)}={\boldsymbol {\hat {p}}}^{(k)}-\left({{\boldsymbol {J}}^{(k)}}^{\text{T}}{\boldsymbol {J}}^{(k)}\right)^{\text{-1}}{{\boldsymbol {J}}^{(k)}}^{\text{T}}{\boldsymbol {f}}^{(k)}} In this equation, p ^ ( k ) {\textstyle {\boldsymbol {\hat {p}}}^{(k)}} is the estimate of p ^ {\textstyle {\boldsymbol {\hat {p}}}} computed in the last iteration. The initial rough estimate is p ^ ( 0 ) {\textstyle {\boldsymbol {\hat {p}}}^{(0)}} . After a refinement step using the foruma above, p ^ ( k + 1 ) {\textstyle {\boldsymbol {\hat {p}}}^{(k+1)}} is the new refined estimate of p ^ {\textstyle {\boldsymbol {\hat {p}}}} . J ( k ) {\textstyle {\boldsymbol {J}}^{(k)}} is the Jacobian matrix (see below). In each update step, the algorithm seeks to minimize the error ‖ f ( k ) ‖ 2 {\textstyle \left\|{\boldsymbol {f}}^{(k)}\right\|_{2}} define by the error vector f ( k ) {\textstyle {\boldsymbol {f}}^{(k)}} . f ( k ) = [ Δ 1 ( k ) − d 1 ⋮ Δ N ( k ) − d N ] {\displaystyle {\boldsymbol {f}}^{(k)}={\begin{bmatrix}\Delta _{1}^{(k)}-d_{1}\\\vdots \\\Delta _{N}^{(k)}-d_{N}\end{bmatrix}}} This error vector contains the distance Δ i ( k ) = ‖ p i − p ^ ( k ) ‖ 2 {\textstyle \Delta _{i}^{(k)}=\left\|{\boldsymbol {p}}_{i}-{\boldsymbol {\hat {p}}}^{(k)}\right\|_{2}} between all anchor node positions p i {\textstyle {\boldsymbol {p}}_{i}} and the current estimate p ^ ( k ) {\textstyle {\boldsymbol {\hat {p}}}^{(k)}} . The Jacobian matrix J ( k ) {\textstyle {\boldsymbol {J}}^{(k)}} of f ( k ) {\textstyle {\boldsymbol {f}}^{(k)}} can be written as follows. J ( k ) = ∂ f ( k ) ∂ p ^ ( k ) T = D ( k ) ( 1 N p ^ ( k ) T − P ) {\displaystyle {\boldsymbol {J}}^{(k)}={\frac {\partial \,{\boldsymbol {f}}^{(k)}}{\partial \,{{\boldsymbol {\hat {p}}}^{(k)}}^{\mathrm {T} }}}={\boldsymbol {D}}^{(k)}\left({\boldsymbol {1}}_{N}{\boldsymbol {{\hat {p}}^{(k)}}}^{\mathrm {T} }-{\boldsymbol {P}}\right)} Here, D ( k ) = d i a g ( Δ 1 ( k ) , … , Δ N ( k ) ) {\textstyle {\boldsymbol {D}}^{(k)}=\mathrm {diag} \left(\Delta _{1}^{(k)},\ldots ,\Delta _{N}^{(k)}\right)} is a diagonal matrix of all Δ i ( k ) {\textstyle \Delta _{i}^{(k)}} and P = [ p 1 … p N ] T {\displaystyle {\boldsymbol {P}}={\begin{bmatrix}{\boldsymbol {p}}_{1}&\dots &{\boldsymbol {p}}_{N}\end{bmatrix}}^{\mathrm {T} }} is a matrix whose rows are the positions of all anchor nodes. The vector 1 N {\displaystyle {\boldsymbol {1}}_{N}} is a column vector of length N {\displaystyle N} that contains only ones.

1. Obtain a rough initial estimate p ^ ( 0 ) {\textstyle {\boldsymbol {\hat {p}}}^{(0)}} of the target node's position.
2. Set k = 0 {\textstyle k=0} .
3. If k {\textstyle k} equals a predefined maximum number iterations, stop the algorithm. In this case, the algorithm did not converge!
4. Compute distances Δ i ( k ) {\textstyle \Delta _{i}^{(k)}} (with i = 1 … N {\displaystyle i=1\ldots N} ) between anchor positions p i {\textstyle {\boldsymbol {p}}_{i}} and the current estimate p ^ ( k ) {\textstyle {\boldsymbol {\hat {p}}}^{(k)}} .
5. Compute the error vector f ( k ) {\textstyle {\boldsymbol {f}}^{(k)}} .
6. If the error ‖ f ( k ) ‖ 2 {\textstyle \left\|{\boldsymbol {f}}^{(k)}\right\|_{2}} is below a prefined threshold, stop the algorithm. Convergence is achieved.
7. Compute the Jacobian J ( k ) {\textstyle {\boldsymbol {J}}^{(k)}} .
8. Solve the equation J ( k ) v ( k ) = f ( k ) {\textstyle {\boldsymbol {J}}^{(k)}{\boldsymbol {v}}^{(k)}={\boldsymbol {f}}^{(k)}} for v ( k ) {\textstyle {\boldsymbol {v}}^{(k)}} using linear least squares method.
   The algebraic solution is the Moore–Penrose inverse: v ( k ) = ( J ( k ) T J ( k ) ) -1 J ( k ) T f ( k ) {\displaystyle {\boldsymbol {v}}^{(k)}=\left({{\boldsymbol {J}}^{(k)}}^{\text{T}}{\boldsymbol {J}}^{(k)}\right)^{\text{-1}}{{\boldsymbol {J}}^{(k)}}^{\text{T}}{\boldsymbol {f}}^{(k)}}
9. Compute the updated position estimate: p ^ ( k + 1 ) = p ^ ( k ) − v ( k ) {\textstyle {\boldsymbol {\hat {p}}}^{(k+1)}={\boldsymbol {\hat {p}}}^{(k)}-{\boldsymbol {v}}^{(k)}} .
10. If the distance ‖ p ^ ( k + 1 ) − p ^ ( k ) ‖ 2 {\textstyle \left\|{\boldsymbol {\hat {p}}}^{(k+1)}-{\boldsymbol {\hat {p}}}^{(k)}\right\|_{2}} between the current and the last estimated target position is below a predefined threshold, stop the algorithm. Convergence is achieved.
11. Increase k {\textstyle k} by one.
12. Go to bullet point 3.

Note:

• This algorithm converges usually within only a few steps.
• The threshold on ‖ f ( k ) ‖ 2 {\textstyle \left\|{\boldsymbol {f}}^{(k)}\right\|_{2}} is not mandatory and can also be left out.
• Step 8 through should be solved numerically instead of using the algebraic solution.

def refine_position_estimate(initial_position_guess, anchor_positions, distances, error_threshold, estimate_threshold, max_iterations):
    assert initial_position_guess.ndim == 1
    assert anchor_positions.ndim == 2
    assert initial_position_guess.shape[0] == anchor_positions.shape[0]
    
    P = anchor_positions
    p_hat = initial_position_guess.copy()
    for iter_idx in range(max_iterations):
        delta = np.linalg.norm(P - p_hat[:, None], axis=0)
        delta = np.maximum(delta, 1e-12)
        f = delta - distances
        error = np.linalg.norm(f)
        if error <= error_threshold:
            print(f"Converged: Error is below threshold: {error}")
            break
        
        J = (p_hat[None, :] - P.T) 
        J /= delta[:, None]
        v = np.linalg.lstsq(J, f, rcond=None)[0]        
        p_hat_diff = np.linalg.norm(v)
        if p_hat_diff <= estimate_threshold:
            print(f"Converged: Distance between last estimate and current estimate is below threshold: {p_hat_diff}")
            break
        p_hat -= v
    else:
        print("WARNING: Algorithm did NOT converge!")
        
    print(f"Number of required iterations: {iter_idx}")
    return p_hat

In this example, there are seven anchor nodes with known positions. The distances between the target node and all anchor nodes are also perfectly known. For this example, the algorithm needs only three iterations to achieve the required accuracy.

anchor_positions = np.array([
    [0.0, 0.0, 0.0],
    [40, 25, 75],
    [0, 80, 0],
    [100, 11, 22],
    [0, 45, 87],
    [0, 0, 100],
    [0, 50, 0]
]).T

true_target_position = np.array([30.0, 40.0, 50.0])
true_distances = np.linalg.norm(anchor_positions - true_target_position[:, None], axis=0)

# An initial rough estimate of the target position.
# This is typically obtained using a (rough) direct estimator or from some prior knowledge.
initial_target_position_estimate = true_target_position + [3, 2, -4]

estimated_target_position = refine_position_estimate_wikpedia(
    initial_position_guess=initial_target_position_estimate,
    anchor_positions=anchor_positions,
    distances=true_distances,
    error_threshold=1e-6,
    estimate_threshold=1e-6,
    max_iterations=1000
)

target_position_error = np.linalg.norm(estimated_target_position - true_target_position)
print()
print(f"True target position:      {true_target_position}")
print(f"Initial estimate:          {initial_target_position_estimate}")
print(f"estimated target position: {estimated_target_position}")
print(f"Achieved accuracy:         {target_position_error:.12f}")

Output:

Converged: Error is below threshold: 1.543685171566139e-09
Number of required iterations: 3

True target position:      [30. 40. 50.]
Initial estimate:          [33. 42. 46.]
estimated target position: [30. 40. 50.]
Achieved accuracy:         0.000000001463

This is a classic celestial (or astronomical) navigation problem, termed the altitude intercept problem (Fig. 3). It is the spherical geometry equivalent of the trilateration method of surveying (although the distances involved are generally much larger). A solution at sea (not necessarily involving the Sun and Moon) was made possible by the marine chronometer (introduced in 1761) and the discovery of the 'line of position' (LOP) in 1837. The solution method now most taught at universities (e.g., U.S. Naval Academy) employs spherical trigonometry to solve an oblique spherical triangle based on sextant measurements of the 'altitude' of two heavenly bodies.^[15][16] This problem can also be addressed using vector analysis.^[17] Historically, graphical techniques – e.g., the intercept method – were employed. These can accommodate more than two measured 'altitudes'. Owing to the difficulty of making measurements at sea, 3 to 5 'altitudes' are often recommended.

As the earth is better modeled as an ellipsoid of revolution than a sphere, iterative techniques may be used in modern implementations.^[18] In high-altitude aircraft and missiles, a celestial navigation subsystem is often integrated with an inertial navigation subsystem to perform automated navigation—e.g., U.S. Air Force SR-71 Blackbird and B-2 Spirit.

While intended as a 'spherical' pseudo range multilateration system, Loran-C has also been used as a 'spherical' true-range multilateration system by well-equipped users (e.g., Canadian Hydrographic Service).^[2] This enabled the coverage area of a Loran-C station triad to be extended significantly (e.g., doubled or tripled) and the minimum number of available transmitters to be reduced from three to two. In modern aviation, slant ranges rather than spherical ranges are more often measured; however, when aircraft altitude is known, slant ranges are readily converted to spherical ranges.^[6]

When there are more range measurements available than there are problem dimensions, either from the same C1 and C2 (or C1, C2 and C3) stations, or from additional stations, at least these benefits accrue:

• 'Bad' measurements can be identified and rejected
• Ambiguous solutions can be identified automatically (i.e., without human involvement) -- requires an additional station
• Errors in 'good' measurements can be averaged, reducing their effect.

The iterative Gauss–Newton algorithm for solving non-linear least squares (NLLS) problems is generally preferred when there are more 'good' measurements than the minimum necessary. An important advantage of the Gauss–Newton method over many closed-form algorithms is that it treats range errors linearly, which is often their nature, thereby reducing the effect of range errors by averaging.^[10] The Gauss–Newton method may also be used with the minimum number of measured ranges. Since it is iterative, the Gauss–Newton method requires an initial solution estimate.

In 3-D Cartesian space, a fourth sphere eliminates the ambiguous solution that occurs with three ranges, provided its center is not co-planar with the first three. In 2-D Cartesian or spherical space, a third circle eliminates the ambiguous solution that occurs with two ranges, provided its center is not co-linear with the first two.

This article largely describes 'one-time' application of the true-range multilateration technique, which is the most basic use of the technique. With reference to Fig. 1, the characteristic of 'one-time' situations is that point P and at least one of C1 and C2 change from one application of the true-range multilateration technique to the next. This is appropriate for surveying, celestial navigation using manual sightings, and some aircraft DME/DME navigation.

However, in other situations, the true-range multilateration technique is applied repetitively (essentially continuously). In those situations, C1 and C2 (and perhaps Cn, n = 3,4,...) remain constant and P is the same vehicle. Example applications (and selected intervals between measurements) are: multiple radar aircraft surveillance (5 and 12 seconds, depending upon radar coverage range), aerial surveying, Loran-C navigation with a high-accuracy user clock (roughly 0.1 seconds), and some aircraft DME/DME navigation (roughly 0.1 seconds). Generally, implementations for repetitive use: (a) employ a 'tracker' algorithm^[19] (in addition to the multilateration solution algorithm), which enables measurements collected at different times to be compared and averaged in some manner; and (b) utilize an iterative solution algorithm, as they (b1) admit varying numbers of measurements (including redundant measurements) and (b2) inherently have an initial guess each time the solution algorithm is invoked.

Hybrid multilateration systems – those that are neither true-range nor pseudo range systems – are also possible. For example, in Fig. 1, if the circle centers are shifted to the left so that C1 is at x 1 ′ = − 1 2 U , y 1 ′ = 0 {\displaystyle x_{1}^{\prime }=-{\tfrac {1}{2}}U,y_{1}^{\prime }=0} and C2 is at x 2 ′ = 1 2 U , y 2 ′ = 0 {\displaystyle x_{2}^{\prime }={\tfrac {1}{2}}U,y_{2}^{\prime }=0} then the point of interest P is at

x ′ = ( r 1 ′ + r 2 ′ ) ( r 1 ′ − r 2 ′ ) 2 U y ′ = ± ( r 1 ′ + r 2 ′ ) 2 − U 2 U 2 − ( r 1 ′ − r 2 ′ ) 2 2 U {\displaystyle {\begin{aligned}x^{\prime }&={\frac {(r_{1}^{\prime }+r_{2}^{\prime })(r_{1}^{\prime }-r_{2}^{\prime })}{2U}}\\[4pt]y^{\prime }&=\pm {\frac {{\sqrt {(r_{1}^{\prime }+r_{2}^{\prime })^{2}-U^{2}}}{\sqrt {U^{2}-(r_{1}^{\prime }-r_{2}^{\prime })^{2}}}}{2U}}\end{aligned}}}

This form of the solution explicitly depends on the sum and difference of r 1 ′ {\displaystyle r_{1}^{\prime }} and r 2 ′ {\displaystyle r_{2}^{\prime }} and does not require 'chaining' from the x ′ {\displaystyle x^{\prime }} -solution to the y ′ {\displaystyle y^{\prime }} -solution. It could be implemented as a true-range multilateration system by measuring r 1 ′ {\displaystyle r_{1}^{\prime }} and r 2 ′ {\displaystyle r_{2}^{\prime }} .

However, it could also be implemented as a hybrid multilateration system by measuring r 1 ′ + r 2 ′ {\displaystyle r_{1}^{\prime }+r_{2}^{\prime }} and r 1 ′ − r 2 ′ {\displaystyle r_{1}^{\prime }-r_{2}^{\prime }} using different equipment – e.g., for surveillance by a multistatic radar with one transmitter and two receivers (rather than two monostatic radars). While eliminating one transmitter is a benefit, there is a countervailing 'cost': the synchronization tolerance for the two stations becomes dependent on the propagation speed (typically, the speed of light) rather that the speed of point P, in order to accurately measure both r 1 ′ ± r 2 ′ {\displaystyle r_{1}^{\prime }\pm r_{2}^{\prime }} .

While not implemented operationally, hybrid multilateration systems have been investigated for aircraft surveillance near airports and as a GPS navigation backup system for aviation.^[20]

This section needs expansion. You can help by adding missing information. (June 2018)

The position accuracy of a true-range multilateration system—e.g., accuracy of the ( x , y ) {\displaystyle (x,y)} coordinates of point P in Fig. 1 -- depends upon two factors: (1) the range measurement accuracy, and (2) the geometric relationship of P to the system's stations C1 and C2. This can be understood from Fig. 4. The two stations are shown as dots, and BLU denotes baseline units. (The measurement pattern is symmetric about both the baseline and the perpendicular bisector of the baseline, and is truncated in the figure.) As is commonly done, individual range measurement errors are taken to be independent of range, statistically independent and identically distributed. This reasonable assumption separates the effects of user-station geometry and range measurement errors on the error in the calculated ( x , y ) {\displaystyle (x,y)} coordinates of P. Here, the measurement geometry is simply the angle at which two circles cross—or equivalently, the angle between lines P-C1 and P-C2. When point P- is not on a circle, the error in its position is approximately proportional to the area bounded by the nearest two blue and nearest two magenta circles.

Without redundant measurements, a true-range multilateration system can be no more accurate than the range measurements, but can be significantly less accurate if the measurement geometry is not chosen properly. Accordingly, some applications place restrictions on the location of point P. For a 2-D Cartesian (trilateration) situation, these restrictions take one of two equivalent forms:

• The allowable interior angle at P between lines P-C1 and P-C2: The ideal is a right angle, which occurs at distances from the baseline of one-half or less of the baseline length; maximum allowable deviations from the ideal 90 degrees may be specified.
• The horizontal dilution of precision (HDOP), which multiplies the range error in determining the position error: For two dimensions, the ideal (minimum) HDOP is the square root of 2 ( 2 ≈ 1.414 {\displaystyle {\sqrt {2}}\approx 1.414} ), which occurs when the angle between P-C1 and P-C2 is 90 degrees; a maximum allowable HDOP value may be specified. (Here, equal HDOPs are simply the locus of points in Fig. 4 having the same crossing angle.)

Planning a true-range multilateration navigation or surveillance system often involves a dilution of precision (DOP) analysis to inform decisions on the number and location of the stations and the system's service area (two dimensions) or service volume (three dimensions).^[21][22] Fig. 5 shows horizontal DOPs (HDOPs) for a 2-D, two-station true-range multilateration system. HDOP is infinite along the baseline and its extensions, as only one of the two dimensions is actually measured. A user of such a system should be roughly broadside of the baseline and within an application-dependent range band. For example, for DME/DME navigation fixes by aircraft, the maximum HDOP permitted by the U.S. FAA is twice the minimum possible value, or 2.828,^[23] which limits the maximum usage range (which occurs along the baseline bisector) to 1.866 times the baseline length. (The plane containing two DME ground stations and an aircraft is not strictly horizontal, but usually is nearly so.) Similarly, surveyors select point P in Fig. 1 so that C1-C2-P roughly form an equilateral triangle (where HDOP = 1.633).

Errors in trilateration surveys are discussed in several documents.^[24][25] Generally, emphasis is placed on the effects of range measurement errors, rather than on the effects of algorithm numerical errors.

• Land surveying using the trilateration method
• Aerial surveying
• Maritime archeology surveying^[26]
• DME/DME RNAV aircraft navigation^[23][27]
• Multiple radar integration (e.g., FAA ERAM)^[28]
• Celestial navigation using the altitude intercept method
• Intercept method—Graphical solution to the altitude intercept problem
• Calibrating laser interferometers^[12]
• SHORAN, Oboe, Gee-H — Aircraft guidance systems developed for 'blind' bombing
• JTIDS (Joint Tactical Information Distribution System) — U.S./NATO system that (among other capabilities) locates participants in a network using inter-participant ranges
• USAF SR-71 Blackbird aircraft — Employs astro-inertial navigation
• USAF B-2 Spirit aircraft — Employs astro-inertial navigation
• Experimental Loran-C technique^[2]

Navigation and surveillance systems typically involve vehicles and require that a government entity or other organization deploy multiple stations that employ a form of radio technology (i.e., utilize electromagnetic waves). The advantages and disadvantages of employing true-range multilateration for such a system are shown in the following table.

True-range multilateration is often contrasted with (pseudo range) multilateration, as both require a form of user ranges to multiple stations. Complexity and cost of user equipage is likely the most important factor in limiting use of true-range multilateration for vehicle navigation and surveillance. Some uses are not the original purpose for system deployment — e.g., DME/DME aircraft navigation.

• Distance geometry problem, similar technique applied to molecules
• Celestial navigation—ancient technique of navigation based on heavenly bodies
• Distance measuring equipment (DME) -- System used to measure distance between an aircraft and a ground station
• Euclidean distance
• Intercept method—Graphical technique used in celestial navigation
• Laser rangefinder
• Multilateration – Addresses pseudo range multilateration
• Rangefinder—Systems used to measure distance between two points on the ground
• Resection (orientation)
• SHORAN—Developed as a military aircraft navigation system, later used for civil purposes
• Surveying
• Tellurometer—First microwave electronic rangefinder
• Triangulation – Surveying method based on measuring angles

• stackexchange.com, PHP / Python Implementation