Issue #339

Simplied code for intersection of two bearings.

Author: rlabbe

kf_book/ukf_internal.py

@@ -238,7 +238,7 @@ def plot_sigmas(sigmas, x, cov):
     pts = sigmas.sigma_points(x=x, P=cov)
     plt.scatter(pts[:, 0], pts[:, 1], s=sigmas.Wm*1000)
     plt.axis('equal')
-    
+
 
 def plot_sigma_points():
     x = np.array([0, 0])
@@ -383,20 +383,38 @@ def plot_scatter_moving_target():
 
 def _isct(pa, pb, alpha, beta):
     """ Returns the (x, y) intersections of points pa and pb
-    given the bearing ba for point pa and bearing bb for
+    given the bearing alpha for point pa and bearing beta for
     point pb.
     """
 
-    B = [pb[0] - pa[0], pb[1] - pa[1]]
-    AB = math.sqrt((pa[0] - pb[0])**2 + (pa[1] - pb[1])**2)
-    ab = atan2(B[1], B[0])
-    a = alpha - ab
-    b = pi - beta - ab
-    p = pi - b - a
+    ax, ay = pa
+    cx, cy = pb
+
+    # bearing to angle
+    alpha = 90 - alpha
+    beta = 90 - beta
+
+    # compute second point, let hypot==1
+    bx = cos(alpha) + ax
+    by = sin(alpha) + ay
+    dx = cos(beta) + cx
+    dy = sin(beta) + cy
+
+    # Line AB represented as a1x + b1y = c1
+    # Line CD represented as a2x + b2y = c2
+
+    a1 = by - ay
+    b1 = ax - bx
+    c1 = a1*ax + b1*ay
+
+    a2 = dy - cy
+    b2 = cx - dx
+    c2 = a2*cx + b2*cy
+    det = a1*b2 - a2*b1
+
+    x = (b2*c1 - b1*c2) / det
+    y = (a1*c2 - a2*c1) / det
 
-    AP = (sin(b) / sin(p)) * AB
-    x = cos(alpha) * AP + pa[0]
-    y = sin(alpha) * AP + pa[1]
     return x, y
 
 
@@ -415,7 +433,7 @@ def _plot_iscts(pos, sa, sb, N=4):
             a_a = actual_angle_a + randn() * math.radians(1)
             a_b = actual_angle_b + randn() * math.radians(1)
 
-            x,y = _isct(sa, sb, a_a, a_b)
+            x, y = _isct(sa, sb, a_a, a_b)
             xs.append(x)
             ys.append(y)