Below is a closed-form linearized solution. It looks for intersection of hyperboloids and makes approximation.
For 3D localisation minimum of 4 anchors (receivers) are required. To solve equation, 0'th anchor is used as reference for linearization. It does not matter which anchor you select for linearization.
\begin{split}
\begin{aligned}
N & \text{ : Number of anchors} \\
Si(x_i,y_i,z_i) & \text{ : The coordinates of anchors (i=0,1,2,3 ... N-1)} \\
S(x,y,z) & \text{ : The coordinate of tag (transmitter)} \\
d_{i} & \text{ : Distance between i'th anchor and tag} \\
d_{0i} & \text{ : Distance difference between 0'th anchor and i'th anchor} \\
\tau _{0i} & \text{ : Time difference of arrival between i'th and 0'th anchors
(i=1,2,3 ... N-1) } \\
c & \text{ : Speed of light/sound }
\end{aligned}
\end{split}
\begin{split}
\tau _{0i} & = \frac{1}{c} |S-S_0| - \frac{1}{c} |S-S_i|\\
& = \frac{1}{c} (d_0 - d_i)
\end{split}
\begin{split}
d_{0i} = \tau _{0i}.c = d_{0} - d_{i}
\end{split}
\begin{equation}
\begin{split}
\Rightarrow d_{i} = d_0 - d_{0i}
\end{split}
\end{equation}
\begin{split}
d_{0}^2 - d_{i}^2 &= |S-S_0|^2 - |S-S_i|^2\\
&=(x-x_0)^2 + (y-y_0)^2 + (z-z_0)^2- (x-x_i)^2 - (y-y_i)^2 - (z-z_i)^2\\
&=x_0^2-x_i^2+2x(x_i-x_0)+y_0^2-y_i^2+2y(y_i-y_0) + z_0^2-z_i^2+2z(z_i-z_0)\\
\end{split}
Substitude $d_i$ for $d_0-d_{0i}$
\begin{equation}
\begin{split}
d_{0}^2 - (d_0 - d_{0i})^2 &= x_0^2-x_i^2 + 2x(x_i-x_0) + y_0^2-y_i^2+2y(y_i-y_0) + z_0^2-z_i^2+2z(z_i-z_0)\\
2d_0.d_{0i} - d_{0i}^2 &= x_0^2-x_i^2 + 2x(x_i-x_0) + y_0^2-y_i^2+2y(y_i-y_0) + z_0^2-z_i^2+2z(z_i-z_0)\\
\end{split}
\end{equation}
Regrouping the terms lead to
\begin{equation}
\begin{split}
x(x_0-x_i) + y(y_0-y_i) + z(z_0-z_i) + d_{0i}.d_0 &= \frac{1}{2}(x_0^2-x_i^2 + y_0^2-y_i^2 + z_0^2-z_i^2 + d_{0i}^2)\\
\end{split}
\end{equation}
These linear system of equations are easily written in matrix form as below. (i = 1,2,3 .. N-1)
\begin{gather}
\begin{bmatrix}
x_0-x_1 & y_0-y_1 & z_0-z_1 & d_{01}\\
x_0-x_2 & y_0-y_2 & z_0-z_2 & d_{02}\\
x_0-x_3 & y_0-y_3 & z_0-z_3 & d_{03}\\
... & ... & ... & ...\\
x_0-x_n & y_0-y_n & z_0-z_n & d_{0n}\\
\end{bmatrix}
\begin{bmatrix}
x\\
y\\
z\\
d_0\\
\end{bmatrix}
=
\begin{bmatrix}
\frac{1}{2}(x_0^{2} - x_1^{2} + y_0^{2} - y_1^{2} + z_0^{2} - z_1^{2} + d_{01}^{2}) \\
\frac{1}{2}(x_0^{2} - x_2^{2} + y_0^{2} - y_2^{2} + z_0^{2} - z_2^{2} + d_{02}^{2}) \\
\frac{1}{2}(x_0^{2} - x_3^{2} + y_0^{2} - y_3^{2} + z_0^{2} - z_3^{2} + d_{03}^{2}) \\
... \\
\frac{1}{2}(x_0^{2} - x_n^{2} + y_0^{2} - y_n^{2} + z_0^{2} - z_n^{2} + d_{0n}^{2}) \\
\end{bmatrix}
\end{gather}
It's an ordinary $A\vec{x}=\vec{b}$ equation. Matrice A and vector $\vec{b}$ are known. The problem requires determination of $\vec{x}$ such that $A\vec{x} \approx \vec{b}$ minimizing sum of squares of residuals. It's called least square regression.
Assuming that error is $\varepsilon$
$\varepsilon(\vec{x}) = A\vec{x} - \vec{b}$
Minimize square of error function $\parallel A\vec{x} - \vec{b}\parallel^2$ by deriving it with respect to $\vec{x}$ and then equate zero.
\begin{equation}
\begin{split}
\frac{\partial}{\partial \vec{x}} \parallel A\vec{x}-\vec{b}\parallel^2 &=0\\
\frac{\partial}{\partial \vec{x}} [(A\vec{x}-\vec{b})^T(A\vec{x}-\vec{b})] &= 0\\
\frac{\partial}{\partial \vec{x}} [\vec{x}^T A^T A \vec{x} - 2\vec{x}^TA^T
\vec{b} + \vec{b}^T \vec{b} ] &= 0\\
2A^T A \vec{x} - 2A^T \vec{b} &= 0 \\
\Rightarrow \vec{x} &= (A^TA)^{-1} A^T\vec{b}
\end{split}
\end{equation}
The vector $\vec{x}$ is solution.
If anchors are not placed uniformly then matrice $A$ is not singular so it can be invertible and solution exists, otherwise QR decomposition can be used. It's another topic.
It's the simplest way for solution. At this point, mostly $\vec{x}$ is used as initial value of further complex iterations. There are lots of different approach for fine localization.
- As you receive help, try to give it too, by answering questions in your area of expertise.
- Take the tour and check the faqs!
- When you see good questions and answers, vote them up by clicking the gray triangles, because the credibility of the system is based on the reputation gained by users sharing their knowledge. Remember to accept the answer, if any, that solves your problem, by clicking the checkmark sign!
– Louis Mar 31 '16 at 14:03