Trilateration using the Levenberg-Marquardt method
True range 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’). — True range multilateration, Wikipedia
We can define a trilateration as an optimization problem to minimize a cost function \(S\) for a given estimate \(\beta\), a tuple of \((x, y, z)\)
$$\begin{aligned}
S(\beta)&=\sum_{i=1}^{N}f_i(\beta)\\
&=\sum_{i=1}^{N}\left[r_i-\sqrt{(x-X_i)^2+(y-Y_i)^2+(z-Z_i)^2}\right]^2
\end{aligned}$$
where \(r_i\) is a measured distnace between an Anchor (i), whose coordinates are \((X_i, Y_i, Z_i)\).
Levenberg-Marquardt method is defined by:
$$\beta_{k+1}=\beta_k-\left(J^TJ+\mu_kdiag\left(J^TJ\right)\right)^{-1}J^Tf(\beta_k)$$
where \(J\) is the Jacobian matrix and \(f\) is a column vector composed of \(f_i\):
$$\begin{aligned}
J&=\begin{bmatrix}
\frac{\partial f_1}{\partial x} & \frac{\partial f_1}{\partial y} & \frac{\partial f_1}{\partial z} \\
\frac{\partial f_2}{\partial x} & \frac{\partial f_2}{\partial y} & \frac{\partial f_2}{\partial z} \\
\vdots & \vdots & \vdots \\
\frac{\partial f_N}{\partial x} & \frac{\partial f_N}{\partial y} & \frac{\partial f_N}{\partial z}
\end{bmatrix}\\
&=\begin{bmatrix}
F_1(x-X_1) & F_1(y-Y_1) & F_1(z-Z_1) \\
F_2(x-X_2) & F_2(y-Y_2) & F_2(z-Z_2) \\
\vdots & \vdots & \vdots \\
F_N(x-X_N) & F_N(y-Y_N) & F_N(z-Z_N)
\end{bmatrix}
\end{aligned}$$
where \(F_i\) is a shorthand of each derivative:
$$F_i=\frac{r_i}{\sqrt{(x-X_i)^2+(y-Y_i)^2+(z-Z_i)^2}}$$
Now, we can estimate \(\beta\) by iterating the equation until it converges.