Fixed diverging UKF filter.

The UKF for the circle tracking was diverging. Also,
I have partially started the section on the ILS, but
much remains to be done on it.

code/ukf_internal.py

@@ -42,6 +42,33 @@ def show_two_sensor_bearing():
     ax.add_artist(circle2)
     plt.show()
 
+
+def show_three_gps():
+    circle1=plt.Circle((-4,0),5,color='#004080',fill=False,linewidth=20, alpha=.7)
+    circle2=plt.Circle((4,0),5,color='#E24A33', fill=False, linewidth=8, alpha=.7)
+    circle3=plt.Circle((0,-3),6,color='#534543',fill=False, linewidth=13, alpha=.7)
+
+    fig = plt.gcf()
+    ax = fig.gca()
+
+#    plt.ylim((-10,10))
+
+    #plt.plot ([-4,0], [0,3], c='#004080')
+    #plt.plot ([4,0], [0,3], c='#E24A33')
+    #plt.text(-4, -.5, "A", fontsize=16, horizontalalignment='center')
+    #plt.text(4, -.5, "B", fontsize=16, horizontalalignment='center')
+
+    ax.add_patch(circle1)
+    ax.add_patch(circle2)
+    ax.add_patch(circle3)
+
+    plt.axis('equal')
+    #ax.set_xlim(-15,15)
+    #ax.set_ylim(-15,15)
+
+    plt.show()
+
+
 def show_sigma_transform():
     fig = plt.figure()
     ax=fig.gca()
@@ -157,6 +184,7 @@ def show_sigmas_for_2_kappas():
 
 
 if __name__ == '__main__':
-    show_sigma_transform()
+    show_three_gps()
+    #show_sigma_transform()
     #show_sigma_selections()
 

experiments/ILS.py

@@ -0,0 +1,141 @@
+# -*- coding: utf-8 -*-
+
+
+import numpy as np
+from numpy.linalg import norm, inv
+from numpy.random import randn
+from numpy import dot
+
+
+numpy.random.seed(1234)
+user_pos = np.array([1000, 100])  # d5, D6
+
+pred_user_pos = np.array([100, 0]) #d7, d8
+
+
+t_pos = np.asarray([[0, 1000],
+                    [0, -1000],
+                    [500, 500]], dtype=float)
+
+
+def transmitter_range(pos, transmitter_pos):
+    """ Compute distance between position 'pos' and the list of positions
+    in transmitter_pos"""
+
+    N = len(transmitter_pos)
+    rng = np.zeros(N)
+
+    diff = np.asarray(pos) - transmitter_pos
+
+    for i in range(N):
+        rng[i] = norm(diff[i])
+
+    return norm(diff, axis=1)
+
+
+
+
+# compute measurement of where you are with respect to seach sensor
+
+
+rz= transmitter_range(user_pos, t_pos) # $B21,22
+
+# add some noise
+for i in range(len(rz)):
+    rz[i] += randn()
+
+
+# now iterate on the predicted position
+pos = pred_user_pos
+
+
+def hx_range(pos, t_pos, r_est):
+    N = len(t_pos)
+    H = np.zeros((N, 2))
+    for j in range(N):
+        H[j,0] = -(t_pos[j,0] - pos[0]) / r_est[j]
+        H[j,1] = -(t_pos[j,1] - pos[1]) / r_est[j]
+    return H
+
+
+def lop_ils(zs, t_pos, pos_est, hx, eps=1.e-6):
+    """ iteratively estimates the solution to a set of measurement, given
+    known transmitter locations"""
+    pos = np.array(pos_est)
+
+    converged = False
+    for i in range(20):
+        r_est = transmitter_range(pos, t_pos) #B32-B33
+        print('iteration:', i)
+        #print ('ra1, ra2', ra1, ra2)
+        print()
+
+        H=hx(pos, t_pos, r_est)
+        
+        Hinv = inv(dot(H.T, H)).dot(H.T)
+
+        #update position estimate
+        y = zs - r_est
+        print('residual', y)
+
+        Hy = np.dot(Hinv, y)
+        print('Hy', Hy)
+
+        pos = pos + Hy
+        print('pos', pos)
+
+        print()
+        print()
+
+        if max(abs(Hy)) < eps:
+            converged = True
+            break
+
+    return pos, converged
+
+
+
+print(lop_ils(rz, t_pos, (900,90), hx=hx_range))
+
+
+
+#####################
+"""
+# compute measurement (simulation)
+rza1, rza2 = transmitter_range(user_pos) # $B21,22
+
+rza1 += randn()
+rza2 += randn()
+
+# now iterate on the predicted position
+pos = pred_user_pos
+
+
+for i in range(10):
+    ra1, ra2 = transmitter_range(pos) #B32-B33
+    print('iteration:', i)
+    print ('ra1, ra2', ra1, ra2)
+    print()
+
+    H = np.array([[-(t1_pos[0] - pos[0]) / ra1, -(t1_pos[1] - pos[1]) / ra1],
+                  [-(t2_pos[0] - pos[0]) / ra2, -(t2_pos[1] - pos[1]) / ra2]])
+    Hinv = inv(H)
+
+    #update position estimate
+    residual_t1 = rza1 - ra1
+    residual_t2 = rza2 - ra2
+    y = np.array([[residual_t1], [residual_t2]])
+    print('residual', y.T)
+
+
+    Hy = np.dot(Hinv, y)
+
+    pos = pos + Hy[:,0]
+    print('pos', pos)
+
+    print()
+    print()
+
+    if (max(abs(y)) < 1.e-6):
+        break
+"""