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)\)


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:


where \(J\) is the Jacobian matrix and \(f\) is a column vector composed of \(f_i\):

\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}
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)

where \(F_i\) is a shorthand of each derivative:


Now, we can estimate \(\beta\) by iterating the equation until it converges.