Solving three simultaneous equations most efficiently for two variables

Question

I am working on a triangulation algorithm to be used with an 8-bit microcontroller using UWB RFID. I will have two unknowns $x$ and $y$ for the item I am tracking, but I need to use three equations to solve it since only using two will give two solutions.

These are the equations:

$$(x-a_{0x})^{2} + (y-a_{0y})^{2} = d_{0}^{2}\\ (x-a_{1x})^{2} + (y-a_{1y})^{2} = d_{1}^{2}\\ (x-a_{2x})^{2} + (y-a_{2y})^{2} = d_{2}^{2}$$

Here $a_{nx}$, $a_{ny}$ and $d_n$ are known.

I have two issues. First, since this will be using RFID, the values that I will use will not give an "exact" value, so the method used to solve this will need to be an approximation.

Second, since this is on an 8-bit processor, I need to use the most efficient method possible to solve this equation set.

Could I have some suggestions on what numerical solving algorithms I should try?

Thanks

EDIT:

So for example, if I have this set of equations as the "true" set: $$(x--6)^{2} + (y-9)^{2} = 25\\ (x-6)^{2} + (y-9)^{2} = 73\\ (x)^{2} + (y)^{2} = 40$$ The exact value of $(x,y)$ is $(-2,6)$.

But since the know values are gathered from my RFID chip, they will never be "exact" and instead, I could have an equation set like:

$$(x--6.01)^{2} + (y-9.6)^{2} = 25.5\\ (x-6.05)^{2} + (y-9)^{2} = 72.8\\ (x)^{2} + (y)^{2} = 40.5$$

So the solving algorithm needs to be able to give a best approximation for the equation. Maybe using ordinary squares?

Answer 1

The difference of any pair of the circle equations is the equation of a line that passes through their intersection points. If the measurements are exact, these three lines are coincident, in which case the common intersection of the three circles can be found by computing the intersection of any two of them. It turns out that in the presence of noise these lines remain coincident, so their intersection can be used as a fairly quick and dirty estimate of the true intersection.

The equation of the line through the intersection points of circles $i$ and $j$ is $$2(a_{ix}-a_{jx})x+2(a_{iy}-a_{jy})y+(d_i^2-a_{ix}^2-a_{iy}^2)-(d_j^2-a_{jx}^2-a_{jy}^2) = 0,$$ or in homogeneous vector notation, $$\begin{align} \mathbf l_{ij} &= [2(a_{ix}-a_{jx}):2(a_{iy}-a_{jy}):(d_i^2-a_{ix}^2-a_{iy}^2)-(d_j^2-a_{jx}^2-a_{jy}^2)] \\ &= [2a_{ix}:2a_{iy}:d_i^2-a_{ix}^2-a_{iy}^2] - [2a_{jx}:2a_{jy}:d_j^2-a_{jx}^2-a_{jy}^2] \\ &= \mathbf m_i - \mathbf m_j. \end{align}$$ The intersection of lines $\mathbf l_{ij}$ and $\mathbf l_{jk}$ is $$\begin{align} \mathbf l_{ij} \times \mathbf l_{jk} &= (\mathbf m_i - \mathbf m_j)\times(\mathbf m_j-\mathbf m_k) \\ &= (\mathbf m_i\times\mathbf m_j)+(\mathbf m_j\times\mathbf m_k)+(\mathbf m_k\times\mathbf m_j). \end{align}$$ It’s not hard to see that the three pairwise intersections yield the same point. To dehomogenize, divide through by the last component of its homogeneous coordinates.

Applying this to your perturbed example produces $$\begin{align} \mathbf m_0 &= [-12.02 : 19.2 : -102.7801] \\ \mathbf m_1 &= [12.1 : 18 : -44.8025] \\ \mathbf m_2 &= [0 : 0 : 40.5] \end{align}$$ and $$(\mathbf m_0\times\mathbf m_1)+(\mathbf m_1\times\mathbf m_2)+(\mathbf m_2\times\mathbf m_0) = [941.2338 : -2759.02526 : -448.68],$$ which dehomogenizes to $(-2.09778, 6.1492)$, which is pretty close to the result in other answers.

Answer 2

Consider the equation to be $$f_i=(x-x_i)^2+(y-y_i)^2-d_i^2$$ What you need to do is to minimize, with respect to $x$ and $y$, $$\Phi=\sum_{i=1}^3 f_i^2$$ and, as usual, you need starting guesses.

To get estimates of them, as already commented by amd, subtract them to get $$2(x_1-x_2)x+2(y_1-y_2) y=(x_1^2+y_1^2-d_1^2)-(x_2^2+y_2^2-d_2^2)\tag 1$$ $$2(x_1-x_3)x+2(y_1-y_3) y=(x_1^2+y_1^2-d_1^2)-(x_3^2+y_3^2-d_3^2)\tag 2$$

Let $$A_1=2(x_1-x_2)\qquad B_1=2(y_1-y_2)\qquad C_1=(x_1^2+y_1^2-d_1^2)-(x_2^2+y_2^2-d_2^2)$$ $$A_2=2(x_1-x_3)\qquad B_2=2(y_1-y_3)\qquad C_2=(x_1^2+y_1^2-d_1^2)-(x_3^2+y_3^2-d_3^2)$$ Solve for $(x,y)$ to get $$x=\frac{B_1\, C_2-B_2\, C_1}{A_2\, B_1-A_1\, B_2}\qquad y=\frac{A_2\, C_1-A_1\, C_2}{A_2 \,B_1-A_1\, B_2}\tag 3$$ Now, back to $\Phi$, compute its partial derivatives $$\frac{\partial \Phi}{\partial x}=4\sum_{i=1}^3 (x-x_i) \left((x-x_i)^2+(y-y_i)^2-d_i^2 \right)$$ $$\frac{\partial \Phi}{\partial y}=4\sum_{i=1}^3 (y-y_i) \left((x-x_i)^2+(y-y_i)^2-d_i^2 \right)$$ and set them equal to $0$. You can solve them using Newton-Raphson method since the first step will give you could estimates.

Using your data, the first step will give $x=-2.09778$ and $y=6.14920$. Doing the second step, you should get $x=-2.03599$ and $y=6.15465$. You can see how close the final results are when compared to the estimates.

Remark

If I may, I would like to comment : what you measure are the $d_i$'s and not their squares. This means that the equations should be instead $$g_i=\sqrt{(x-x_i)^2+(y-y_i)^2}-d_i$$ and you would need to to minimize, with respect to $x$ and $y$, $$\Psi=\sum_{i=1}^3 g_i^2$$ which is more tedious but perfectly doable. $$\frac{\partial \Psi}{\partial x}=2\sum_{i=1}^3 \frac{(x-x_i) \left(\sqrt{(x-x_i)^2+(y-y_i)^2}-d_i\right)}{\sqrt{(x-x_i)^2+(y-y_i)^2}}$$ $$\frac{\partial \Psi}{\partial y}=2\sum_{i=1}^3 \frac{(y-y_i) \left(\sqrt{(x-x_i)^2+(y-y_i)^2}-d_i\right)}{\sqrt{(x-x_i)^2+(y-y_i)^2}}$$

For the estimates, keep the previous procedure. Working with $\Psi$, you should get $x=-2.08559$ and $y=6.16485$. Not very different but not the same and more consistent with the principle of data reconciliation (which is what you are doing).

Answer 3

Based on your comment, you want a best estimate point for the intersection when there is really no actual intersection

---

You can try to adjust the radii of the circles until you have an actual common intersection point.

Modify the set of equations to include $k$ as shown below: $$\begin{align} (x+6.01)^2+(y-9.6)^2&=\left(k+\sqrt{25.5}\right)^2\\ (x-6.05)^2+(y-9)^2&=\left(k+\sqrt{72.8}\right)^2\\ x^2+y^2&=\left(k+\sqrt{40.5}\right)^2 \end{align}$$

Solving for $k,x$ and $y$, you get two solutions, but only one is meaningful: $$\{\{k\to -13.3504,x\to 1.78746,y\to 6.75391\},\color{red}{\{k\to 0.140533,x\to -2.13868,y\to 6.14284\}\}}$$

This is quick and doesn't really involve calculus

---

The next method is tedious, and maybe redundant.

We define: $$g_1(x,y):=(x+6.01)^2+(y-9.6)^2-25.5\\ g_2(x,y):=(x-6.05)^2+(y-9)^2-72.8\\ g_3(x,y):=x^2 + y^2 - 40.5$$

Try to solve the different intersection points of two circles closest to the expected intersection point of the 3 circles, i.e: $$\begin{align} \text{Solve} &g_1(x,y)=0\land g_2(x,y)=0\\ &g_1(x,y)=0\land g_3(x,y)=0\\ &g_2(x,y)=0\land g_3(x,y)=0\\ \end{align}$$

You get these points, however, only those in red is relevant: $$g_1(x,y)\land g_2(x,y): \{\color{red}{\{x\to -2.08412,y\to 6.42393\}},\{x\to -1.78826,y\to 12.3707\}\}\\ g_1(x,y)\land g_3(x,y):\{\{x\to -3.9506,y\to 4.98927\},\color{red}{\{x\to -2.76216,y\to 5.73328\}}\}\\ g_2(x,y)\land g_3(x,y):\{\color{red}{\{x\to -1.95803,y\to 6.05526\}},\{x\to 6.34637,y\to 0.472857\}\}$$

Now you want to find $(x_i,y_i)$ where the total sum of the lengths from each of the relevant point above to $(x_i,y_i)$ is minimized. Define: $$a_x=-1.95803,a_y=6.05526\\ b_x=-2.08412,b_y=6.42393\\ c_x=-2.76216,c_y=5.73328$$

Now we want to minimize: $$m(x,y):=\sqrt{\left(x-a_x\right){}^2+\left(y-a_y\right){}^2}+\sqrt{\left(x-b_x\right){}^2+\left(y-b_y\right){}^2}+\sqrt{\left(x-c_x\right){}^2+\left(y-c_y\right){}^2}$$ Using numerical methods, we get that $m(x,y)$ (at $m(x,y)=1.2207$) is minimized when: $$\{x\to -2.08342,y\to 6.12726\}$$

Now we see that the first method is fairly close to this numerical answer.

Answer 4

In the first place, you should work with the pairwise subtracted equations, as they reduce to linear ones and make the solution unique, while sparing square roots. You will get three straight lines approximately coincident.

For a simple estimate, I would take the intersection of the two lines that are the less parallel. To check this, you can compare the cosine of the angle between normals,

$$\frac{a_ia_j+b_ib_j}{\sqrt{a_i^2+b_i^2}\sqrt{a_j^2+b_j^2}}$$ (assuming equations of the form $ax+by+c=0$) and pick the smallest.

In other words, and to avoid the square roots, take the smallest of

$$(a_ia_j+b_ib_j)^2(a_k^2+b_k^2).$$

---

If you want to "improve" the value by least-squares, a possible formulation is to minimize the total discrepancy between the radii and the distances to the centers.

$$\sum_k\left(\sqrt{(x-x_k)^2+(y-y_k)^2}-d_k\right)^2.$$

This leads to a nasty system of two nonlinearequations $$\begin{cases}\sum_k\dfrac{x-x_k}{\sqrt{(x-x_k)^2+(y-y_k)^2}}\left(\sqrt{(x-x_k)^2+(y-y_k)^2}-d_k\right)=0,\\ \sum_k\dfrac{y-y_k}{\sqrt{(x-x_k)^2+(y-y_k)^2}}\left(\sqrt{(x-x_k)^2+(y-y_k)^2}-d_k\right)=0 \end{cases}$$

which I don't think should be used.

Without much loss, it can be approximated as

$$\begin{cases}\sum_k\dfrac{x-x_k}{d_k}\left(\sqrt{(x-x_k)^2+(y-y_k)^2}-d_k\right)=0,\\ \sum_k\dfrac{y-y_k}{d_k}\left(\sqrt{(x-x_k)^2+(y-y_k)^2}-d_k\right)=0 \end{cases}$$

or

$$\begin{cases}\sum_k\dfrac{x-x_k}{d_k}\dfrac{(x-x_k)^2+(y-y_k)^2-d_k^2}{d_k}=0,\\ \sum_k\dfrac{y-y_k}{d_k}\dfrac{(x-x_k)^2+(y-y_k)^2-d_k^2}{d_k}=0 \end{cases}$$

but this doesn't seem to be enough to make it tractable.

IMO, this approach is a dead end due to its computational complexity vs. the modest gain in accuracy.