# Bearing landmark localization

I am trying to initialize the position of a set of landmarks using only bearing observations. Each robot pose, described by position (x,y) and orientation (theta), observes a small set of landmarks. Each observation, bearing angle, is expressed in the robot frame. My idea was to consider pair of consecutive robot poses, retrieve the equation of the lines passing though them and make the intersection to obtain the position of the landmark observed by both poses. Using the robot position and the bearing observation, I can retrieve the equation of a line as y-y1=m(x-x1) where the slope m can be computed as m=tan(bearing).

To check if this is the right way to proceed I tested the method on the data from the ground truth dataset. For instance, two consecutives poses R1=(1.0384, 0.037015, 0.0057897) and R2=(2.0171, 0.019962, 0.0017791) both observe the landmark L1 in position (2.8116, -4.2695), respectively with bearing values of B1=-1.1860 and B2=-1.3894. Evaluating the lines passing through R1-L1 and R2-L1 their equation is y1=-2.4287x + 2.5589 and y2=-5.3989x+10.9102. If now I try to retrieve the slope of the two lines using the method mentioned before I obtain different values m1=-2.4692 and m2=-5.4522.

Some final notes:

• I'm almost completely sure that the bearing observation provided is the actual "mathematical" angle measurement, hence no transformation of it is required.
• I think that I should use also the theta angle of the robot pose in some way, but I'm not sure how.

What am I missing?

So the method to figure of something with only bearing measurements is called triangulation. While I think your method works you can much more easily solve it using sin rule.

Given at least 1 side and 2 angles you can figure out all the properties of a triangle using the sin rule.

$$\frac{a}{sin(a)}=\frac{c}{sin(C)}=\frac{b}{sin(B)}$$

In your problem you have a length(distance between poses), and 2 angles(bearing measurements). So you have all the information you need to figure out the position of the landmark.

I think that I should use also the theta angle of the robot pose in some way, but I'm not sure how.

Correct. It is pretty simple. You use it to convert the local bearing measurement into the global frame.

$$\theta_{bG}=\theta_R+\theta_{bmeas}$$

So the global angle bearing measurement($$\theta_{bG}$$) is the heading($$\theta_R$$) of your robot plus your bearing measurement($$\theta_{bmeas}$$).

The global bearing measurement is what you plug into the sin rule equations.

• Even following the sine rule produces different results. For instance, using again the ground truth with the previous data, I can compute the distances between each point b = 0.97885, c = 4.6573 and a = 4.3624. Now, the global bearing measurement are A = -1.1802, C = -3.1416 - (-1.3854 + 0.0017791) = -1.7580 (in this case the bearing measurement is the exterior angle so I have to find the adjacent interior one) and B = 3.1416 - A - C = - 0.20339 (after having it normalized between [-180, 180]). By applying the sine rule for c = b * sin(C) / sin(B) = 4.7614 which is slightly different. – R.Go Jul 10 '20 at 8:53
• It does work. In your case since you are using such significant digits you have to be extra careful. To verify I would just use some nice integer numbers as coordinates. I believe also in your case you are using the wrong bearing measurements. Your bearings are already in the global frame. I assume you just compute it using the positions and arctan2. So doing the extra frame conversion step would actually be wrong. – edwinem Jul 10 '20 at 17:46