How to estimate position from noisy angles?

Question

First of all: I think this problem has been studied very well (I assume in the field of surveying or navigation), but I was unable to find any information as I do not know what it is called.

EDIT: As @Rahul mentioned this is called resection problem. Wikipedia points to various methods but they usually require exact measurements and assume that you have $n=3$.

---

Suppose we know the exact position of a number $n \geq 3$ points (in the example below $n=4$) $P_i$ in the plane. (We can assume that those points are in some non degenerate configuration.) Furthermore we are at some unknown location $P_0$ looking in an unknown direction $\hat x_1$. Now we can observe the angles $\alpha_i$ of how much the direction we see each point deviates from our direction $\hat x_1$.

With these observations $\alpha_1,\ldots,\alpha_n$ we'd like to determine $P_0$ (as well as the direction $\hat x_1$, but that is trivial once we know $P_0$). The problem is that the angles $\alpha_i$ are affected by noise i.e. cannot be measured exactly. (We can assume that they have an e.g. normally distributed error, or whatever is convenient. )

Is there a way to "robustly" estimate $P_0$?

Is there a well known name for this problem?

Answer 1

Least Squares via Inscribed Angle theorem

This is an algorithm I came up with but I'm not sure whether it is actually used or how well it works. I will provide an implementation for comparision in the future.

First consider the case where we have exact measurements:

Given the angles $\beta_{ij} = \alpha_{i}-\alpha_{j}$ we can construct a circle $c_{ij}$ of all points under which the line segment defined by $P_i$ and $P_j$ is visible under the angle $\beta_{ij}$ with the inscribed angle theorem.

For example in the special case where $\beta = \pi/2$, then this circle has center $\frac{1}{2}(P_i + P_j)$ and radius $\Vert P_i - P_j \Vert /2$. (This can be explained using the Thales Theorem as a special case of the inscribed angle theorem.)

If we take a set of such circles we know that $P_0$ must be in their intersection.

Here is a diagram of two of these circles for the arbitrary case of $\beta_{12} = 45^\circ$ and $\beta_{23} = -140^\circ$.

The center and radius of circle $c_{ij}$ is fully defined by a function of $P_i$, $P_j$, and $\beta_{ij}$ (via the inscribed angle theorem). (see appendix) Similarly, $c_{jk}$ is fully defined by $P_j$, $P_k$, and $\beta_{jk}$. The intersection of $c_{ij}$ and $c_{jk}$ is thus $\{P_j, P_0\}$, so the ambiguity is resolved (just select the point that isn't $P_j$). This fails however in the degenerate case of all $P$ being on the same circle (in which case they will all yield the same redundant equation - imagine if $P_k$ in the above diagram was on the orange $c_{ij}$). In general, the intersection of two circles is determined by solving the linear system formed by subtracting their equations.

This whole process is also described here.

Let us now consider the noisy case:

In this case the $n$ circles, and the $n$ linear equations we get will not necessarily intersect in a single point, so we cannot directly solve the system of linear equations. But instead we can consider it as a least squares problem, to find a point that is close to the pairwise intersections.

Of course, this isn't optimal in any probabilistic sense with regard to the relative noises of the various bearing measurements. For example, in a practical setting, a very far away landmark might be harder to accurately determine bearing to since it is harder to see. If this is to be accounted for, note that the measurement noise will enter the equations nonlinearly (the transformation from bearings to circles via the inscribed angle theorem is nonlinear) and will require an approximation like the answer given by @jnez71.

---

Appendix: Construction of the circles

Case A: $\beta_{ij} \leq \pi/2$

Let us assume ssuming $P_0$, $P_i$, $P_j$ are in positive orientation.

Here the inscribed angle theorem tells us $\gamma = 2\beta_{ij}$ (in the picture $i=1, j=2$). So we can compute $M_{ij} = \frac{1}{2}(P_i + P_j)$ and therefore $\angle (M_{ij}, C, P_i) = \gamma /2$. Let $R$ be a rotation matrix of $90^\circ$ $$R = \begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix}$$. Then we can write

$$C = M_{ij} + t R \frac{(P_j - P_i)}{\Vert P_j - P_i \Vert}$$

where $t$ is the signed distance from $M_{ij}$ to $C$,

and then when considering the trangle $C,M_{ij},P_j$ we see that

$$\frac{t}{\Vert P_j - M_{ij} \Vert} = \tan(\pi/2 - \gamma/2)$$

which we can solve for $t$ which then yields $C$. Then the radius $r = \Vert C - P_i \Vert$ and we can easily write down the equation of the circle.

Case B: $\beta_{ij} \geq \pi/2$

Since we used $t$ as a signed distance (hence the assumption about the positive orientation), we can also apply the same formula from the first case.

Answer 2

Each of the $\ i=1,2,...,n\ $ bearing measurements can be expressed as, $$ \alpha_i = \angle\delta_i - \alpha_0 $$ where $\alpha_0$ is the unknown angle from $x_1$ to $\hat{x}_1$ and $$ \delta_i := P_i-P_0\\ \angle\delta_i = \tan^{-1}({\delta_i}_2\ /\ {\delta_i}_1) $$

If we define, $$ q := \begin{bmatrix} P_0 \\ \alpha_0 \end{bmatrix}\ \ \ \ \ \ z := \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \vdots \\ \alpha_n \end{bmatrix} $$ and assume our bearing sensor is subject to additive noise then we can write, $$ z = h(q) + \epsilon $$ as our complete measurement model, where $\epsilon$ is a random variable with some known characteristics, like perhaps zero mean and covariance $R$.

It would also be nice / common to have some "prior" probabilistic information about our state $q$. That is, even if we don't know it exactly, we do have a hunch about what it could be, described by a probability distribution $q \sim \rho(q)$.

Unfortunately, I don't see a way to reformulate $h(q)$ to be linear in $q$, so I can't just provide you with the closed-form optimal solution: i.e. the $q$ that maximizes the conditional probability distribution $\rho(q|z)$. As far as I can tell, we can only approximate this solution using the general approaches of nonlinear estimation theory.

I implemented an extended Kalman filter as a first attempt. This involves a linearization of $h(q)$, so I computed its Jacobian by hand, $$ \frac{dh_i}{dq} = \begin{bmatrix} \frac{{\delta_i}_2}{||\delta_i||^2_2} & \frac{{-\delta_i}_1}{||\delta_i||^2_2} & -1\end{bmatrix} $$

Another thing to note is that since we are dealing with angles, our measurements (and part of the state) are really members of $\mathbb{SO}2$ not $\mathbb{R}$. Thus we must be careful to work in the Lie algebra, which for this simply means "unwrapping" any angle differences onto $[-\pi, \pi]$.

To fully realize your problem, I have assumed that the additive bearing sensor noise is Gaussian with zero mean and a standard deviation of $5$ degrees. I arbitrarily placed $7$ landmarks. The animation below shows the EKF converging. I start with an incorrect state guess and a large covariance to reflect my prior uncertainty. You can also see that the bearing measurements (yellow rays) don't quite hit their respective landmarks (green dots). Obviously the "process model" in this case is stationary (the true state isn't moving). I linearize about the first guess, compute the best linear estimator (i.e. EKF corrector step), then relinearize about the updated mean and "reset" my covariance back to the prior before attempting another linearization & update. To be clear, only $7$ measurements total were taken (one for each landmark), and all $7$ are incorporated at the same time - the "iterations" are just relinearizations. The code is here. If you have any questions feel free to ask.

Here is one more visualization this time giving the bearing sensor rediculous noise - a standard deviation of $30$ degrees (you can see how way-off the yellow rays are). The filter still does a good job, with its posterior covariance reasonably reflecting the direction along which we still have uncertainty. Note that heading always converges well because the sensor model is actually linear in that state.