# 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.