My goal is to implement algorithm for fine TDOA multilateration. I calculate the initial guess in a way similar to this answer (it holds that $D_i = D_0 + d_{0i}$ instead of $D_i = D_0 - d_{0i}$). However that answer and next answer suggest to use non-linear regression for fine localization. I have come up with a model
$d_{0i} + D_{0} = \sqrt{(x_i - X)^2 + (y_i - Y)^2} $
where
- $d_{0i}$ is the distance difference between zeroth anchor and i'th anchor which is given
- $D_0$ is the distance between zero'th anchor and tag to be found
- $x_i$ and $y_i$ are coordinates of the i'th anchor that are given, and
- $X$ and $Y$ are coordinates of the tag to be found.
This is how it looks on paper.
I would like to learn how I can use this model to get the final D0, X and Y coordinates. Any tips on how to implement this model/tips on resources is welcome - I guess I have to define a cost function (I don't know how to do that for this model) and then use some minimizer to get the results, but I may be wrong.
You want to minimize in the least square sense, $$\Phi=\frac12\sum_{i=1}^n \left(\sqrt{(x_i - X)^2 + (y_i - Y)^2}-(d_i+D)\right)^2$$ which is difficult unless you have good guesses.
To get them, consider the $n$ equations $$f_i=(x_i - X)^2 + (y_i - Y)^2-(d_i+D)^2=0$$ and build the $\frac {n(n-1)}2$ equations which, after expanding, write $$f_j-f_i=2(x_i-x_j) X+2(y_i-y_j) Y+2(d_i-d_j) D=(x_i^2-x_j^2)+(y_i^2-y_j^2)-(d_i^2-d_j^2)$$ and consider that this is a multilinear regression without intercept (very simple to solve using matrices).
So, after this preliminary step, we have the estimates of $(X,Y,D)$ and we could minimize $\Phi$. Naming $$r_i=\sqrt{(x_i - X)^2 + (y_i - Y)^2}-(d_i+D)\implies \Phi=\frac12\sum_{i=1}^n r_i^2$$ and then the equations to be solved are $$\frac{\partial \Phi}{\partial X}=\sum_{i=1}^n r_i \frac{\partial r_i}{\partial X}=0 \quad\qquad \frac{\partial \Phi}{\partial Y}=\sum_{i=1}^n r_i \frac{\partial r_i}{\partial Y}=0 \quad\qquad\frac{\partial \Phi}{\partial D}=\sum_{i=1}^n r_i \frac{\partial r_i}{\partial D}=0 $$ where $$\frac{\partial r_i}{\partial X}=\frac{X-x_i}{\sqrt{(x_i-X)^2+(y_i-Y)^2}}$$ $$\frac{\partial r_i}{\partial Y}=\frac{Y-y_i}{\sqrt{(x_i-X)^2+(y_i-Y)^2}}$$ $$\frac{\partial r_i}{\partial D}=-1$$ and to solve the three equations$$\frac{\partial \Phi}{\partial X}=\frac{\partial \Phi}{\partial Y}=\frac{\partial \Phi}{\partial D}=0$$ use Newton-Raphson method starting from the guesses $(X_0,Y_0,D_0)$ given by the preliminary step. To make life easier, use numerical derivatives.
This should work.