OrbitFG now working with issue 2 fixed.

Author: Clark Taylor

OrbitFG.py

@@ -293,7 +293,7 @@ def opt(self):
             delta_x = spla.spsolve(M,Lty)
             scale = 1
             # Damp the Gauss-Newton step if it doesn't do what the linearization predicts
-            scale_good = False
+            scale_good = la.norm(delta_x) < 10 # if the first step is too small, just do it and don't even check
             while not scale_good:
                 next_y = self.create_y(self.add_delta(delta_x*scale))
                 pred_y = y-L.dot(delta_x*scale)
@@ -350,26 +350,26 @@ def test_Jacobian(batch_uni, col, dx = .001):
     opt_class.opt()
     plt.plot(opt_class.states[:,0],opt_class.states[:,1],'b',label='opt')
     plt.legend()
-    plt.savefig(f'{prefix}_res.png')
+    plt.savefig(f'{prefix}_res_new.png')
 
     plt.figure()
     plt.plot(opt_class.states[:,0],opt_class.states[:,1],c='b', label='estimate')
     plt.plot(truth[:,0],truth[:,1],'r--',label='truth')
     plt.legend()
     ax=plt.gca()
     ax.set_aspect('equal')
-    plt.savefig('FG_'+prefix+'.png')
+    plt.savefig('FG_'+prefix+'_new.png')
     plt.show()
 
 
     plt.figure()
     plt.plot(opt_class.states-truth)
     plt.legend (['x','y','vx','vy'])
     plt.title('errors')
-    plt.savefig(f'{prefix}_errors.png')
+    plt.savefig(f'{prefix}_errors_new.png')
     plt.show()
 
-    np.savez('fg_'+prefix+'_res',fg_res=opt_class.states, truth=truth)
+    np.savez('fg_'+prefix+'_res_new',fg_res=opt_class.states, truth=truth)