title: Propagated Solution of Lambert Solver Leads to Wrong Orbit
body: <p>Excuse me for the length of the title please but this is a pretty specific question. I'm currently simulating a launch of a rocket to mars in the 2022 launch window and I noticed that my rocket is a far distance away from Mars, even though it's traveling in the right direction. After simplifying my code to narrow down the problem, I simply plotted the orbits of the Earth and Mars (Using data from NASA's SPICE library) and propagated the position and velocity given to me by the lambert solver I implemented (Universal variables) to plot the final orbit of the rocket.</p>
  <p>I'm only letting the Sun's gravity effect the rocket, not the Earth or Mars, to minimize my problem space. Yet even though I've simplified my problem so far, the intersection between Mars' and my rocket's orbits happens well before the time of flight has been simulated all the way, and the minimum distance between the two bodies is more than a million kilometers at all times.</p>
  <p><a href="https://i.stack.imgur.com/sBGVf.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/sBGVf.png" alt="Orbits" /></a></p>
  <p>That being said, something must be wrong but I cannot find the problem. I've made sure the lambert solver code I copied is correct by comparing it to Dario Izzo's method and both gave the same results. Furthermore, I've also checked that my orbit propagator works by propagating Mars' and the Earth's orbits and comparing those ellipses to the data from SPICE.</p>
  <p>In conclusion, I assume this must be a stupid little mistake I made somewhere, but cannot find because I lack experience in this field. Thank you for any help! :)</p>
  <p>This is the JupyterLab notebook I used:</p>
  <pre><code>import numpy as np
  import matplotlib.pyplot as plt
  import json
  import math
  import spiceypy as spice
  
  # Physics
  G = 6.6741e-11
  
  class Entity:
      def __init__(self, x, v, m, do_gravity):
          self.x = x
          self.v = v
          self.a = np.array([0,0,0])
          self.m = m
          self.do_gravity = do_gravity
  
      def apply_force(self, f):
          self.a = np.add(self.a, f / self.m);
  
      def step(self, dt):
          self.v = np.add(self.v, self.a * dt)
          self.x = np.add(self.x, self.v * dt)
          self.a = np.array([0,0,0])
  
  class StaticEntity(Entity):
      def __init__(self, body, t, do_gravity):
          super().__init__(self.get_state(body, t)[:3], self.get_state(body, t)[3:], self.get_mass(body), do_gravity)
          self.body = body
          self.t = t
  
      def step(self, dt):
          self.t += dt
          state = self.get_state(self.body, self.t)
          self.x = state[:3]
          self.v = state[3:]
  
      @classmethod
      def get_state(self, body, t):
          [state, lt] = spice.spkezr(body, t, &quot;J2000&quot;, &quot;NONE&quot;, &quot;SSB&quot;)
          return state * 1000
  
      @classmethod
      def get_mass(self, body):
          [dim, gm] = spice.bodvrd(body, &quot;GM&quot;, 1)
          return gm * 1e9 / G
  
      def get_position(self, t):
          return self.get_state(self.body, t)[:3]
  
      def get_velocity(self, t):
          return self.get_state(self.body, t)[3:]
  
  class Propagator:
      def __init__(self, entities):
          self.entities = entities
  
      def step(self, dt):
          for e1 in self.entities:
              for e2 in self.entities:
                  if (e1 is e2) or (not e1.do_gravity) or isinstance(e2, StaticEntity):
                      continue
  
                  diff = np.subtract(e1.x, e2.x)
                  fg = G * e1.m * e2.m / np.dot(diff, diff)
                  force = fg * diff / np.linalg.norm(diff)
  
                  e2.apply_force(force)
  
          for entity in self.entities:
              entity.step(dt)
  
  
  # Lambert solver
  
  def C2(psi):
      if psi &gt;= 0.0:
          sp = math.sqrt(psi)
          return (1 - math.cos(sp)) / psi
      else:
          sp = math.sqrt(-psi)
          return (1 - math.cosh(sp)) / psi
  
  def C3(psi):
      if psi &gt;= 0.0:
          sp = math.sqrt(psi)
          return (sp - math.sin(sp)) / (psi * sp)
      else:
          sp = math.sqrt(-psi)
          return (sp - math.sinh(sp)) / (psi * sp)
  
  def lambert_solve(r1, r2, tof, mu, iterations, tolerance):
      R1 = np.linalg.norm(r1)
      R2 = np.linalg.norm(r2)
  
      cos_a = np.dot(r1, r2) / (R1 * R2)
      A = math.sqrt(R1 * R2 * (1.0 + cos_a))
  
      sqrt_mu = math.sqrt(mu)
  
      if A == 0.0:
          return None
  
      psi = 0.0
      psi_lower = -4.0 * math.pi * math.pi
      psi_upper =  4.0 * math.pi * math.pi
  
      c2 = 1.0 / 2.0
      c3 = 1.0 / 6.0
  
      for i in range(iterations):
          B = R1 + R2 + A * (psi * c3 - 1.0) / math.sqrt(c2)
  
          if A &gt; 0.0 and B &lt; 0.0:
              psi_lower += math.pi
              B = -B
  
          chi = math.sqrt(B / c2)
          chi3 = chi * chi * chi
  
          tof_new = (chi3 * c3 + A * math.sqrt(B)) / sqrt_mu
  
          if math.fabs(tof_new - tof) &lt; tolerance:
              f = 1.0 - B / R1
              g = A * math.sqrt(B / mu)
              g_dot = 1.0 - B / R2
  
              v1 = (r2 - f * r1) / g
              v2 = (g_dot * r2 - r1) / g
  
              return (v1, v2)
  
          if tof_new &lt;= tof:
              psi_lower = psi
          else:
              psi_upper = psi
  
          psi = (psi_lower + psi_upper) * 0.5
          c2 = C2(psi)
          c3 = C3(psi)
  
      return None
  
  
  # Set up solar system
  spice.furnsh('solar_system.tm')
  
  inject_time = spice.str2et('2022 Sep 28 00:00:00')
  exit_time = spice.str2et('2023 Jun 1 00:00:00')
  
  sun   = StaticEntity(&quot;Sun&quot;,             inject_time, True)
  earth = StaticEntity(&quot;Earth&quot;,           inject_time, False)
  mars  = StaticEntity(&quot;Mars Barycenter&quot;, inject_time, False)
  
  (v1, v2) = lambert_solve(earth.get_position(inject_time), mars.get_position(exit_time), exit_time - inject_time, G * sun.m, 1000, 1e-4)
  rocket = Entity(earth.x, v1, 100000, False)
  
  propagator = Propagator([sun, earth, mars, rocket])
  
  # Generate data
  earth_pos  = [[], [], []]
  mars_pos   = [[], [], []]
  rocket_pos = [[], [], []]
  
  t = inject_time
  dt = 3600 # seconds
  
  while t &lt; exit_time:
      propagator.step(dt)
  
      earth_pos[0].append(earth.x[0])
      earth_pos[1].append(earth.x[1])
      earth_pos[2].append(earth.x[2])
  
      mars_pos[0].append(mars.x[0])
      mars_pos[1].append(mars.x[1])
      mars_pos[2].append(mars.x[2])
  
      rocket_pos[0].append(rocket.x[0])
      rocket_pos[1].append(rocket.x[1])
      rocket_pos[2].append(rocket.x[2])
  
      t += dt
  
  # Plot data
  
  plt.figure()
  plt.title('Transfer orbit')
  plt.xlabel('x-axis')
  plt.ylabel('y-axis')
  plt.plot(earth_pos[0], earth_pos[1], color='blue')
  plt.plot(mars_pos[0], mars_pos[1], color='orange')
  plt.plot(rocket_pos[0], rocket_pos[1], color='green')
  </code></pre>
  <p>EDIT:</p>
  <p>I recently remodeled my code so that it uses orbit class to represent the entities. This actually gave me acceptable results, even though the code is, in theory, not doing anything differently (as far as I can tell; obviously something must be different)</p>
  <pre><code>def norm(a):
      return np.dot(a, a)**0.5
  
  def fabs(a):
      return -a if a &lt; 0 else a 
  
  def newton_raphson(f, f_dot, x0, n):
      res = x0
  
      for i in range(n):
          res = res - f(res) / f_dot(res)
  
      return res
  
  def get_ephemeris(body, time):
      state, _ = sp.spkezr(body, time, &quot;J2000&quot;, &quot;NONE&quot;, &quot;SSB&quot;)
      return np.array(state[:3]) * ap.units.km, np.array(state[3:]) * ap.units.km / ap.units.s
  
  def get_mu(body):
      _, mu = sp.bodvrd(body, &quot;GM&quot;, 1)
      return mu * ap.units.km**3 / ap.units.s**2
  
  class orbit:
      def __init__(self, position, velocity, mu):
          self.position = position
          self.velocity = velocity
          self.mu = mu
  
      @staticmethod
      def from_body(name, center, time):
          return static_orbit(name, center, time)
  
      def get_ephemerides(self, t, dt):
          time = 0
          positions = []
          velocities = []
          #M = self.M
          position = self.position
          velocity = self.velocity
  
          delta_t = dt * ap.units.s
          t1 = t * ap.units.s
  
          while time &lt; t1:
              g = self.mu / np.dot(position, position)
              g_vec = g * -position / norm(position)
              velocity = np.add(velocity, g_vec * delta_t)
              position = np.add(position, velocity * delta_t)
  
              positions.append(position)
              velocities.append(velocity)
  
              time = time + delta_t
  
          return positions, velocities
  
  class static_orbit(orbit):
      def __init__(self, name, center, time):
          p, v = get_ephemeris(name, time)
          pc, vc = get_ephemeris(center, time)
  
          super().__init__(p - pc, v - vc, get_mu(center))
  
          self.name = name
          self.center = center
          self.time = time
  
      def get_ephemerides(self, t, dt):
          time = 0
          positions = []
          velocities = []
  
          while time &lt; t:
              p, v = get_ephemeris(self.name, time + self.time)
              pc, vc = get_ephemeris(self.center, time + self.time)
  
              positions.append(p - pc)
              velocities.append(v - vc)
  
              time += dt
  
          return positions, velocities
  
  sp.furnsh('solar_system.tm')
  
  t1 = sp.str2et('2022 Sep 28 00:00:00')
  t2 = sp.str2et('2023 Jun 10 00:00:00')
  
  eo = orbit.from_body(&quot;Earth&quot;, &quot;Sun&quot;, t1)
  mo = orbit.from_body(&quot;Mars Barycenter&quot;, &quot;Sun&quot;, t1)
  
  earth_x, earth_v = eo.get_ephemerides(t2 - t1, 3600)
  mars_x, mars_v = mo.get_ephemerides(t2 - t1, 3600)
  
  l = lambert(earth_x[0], mars_x[-1], t2 - t1, get_mu(&quot;Sun&quot;), 1000, 1e-6)
  
  ro = orbit(earth_x[0], l.v1, get_mu(&quot;Sun&quot;))
  
  rocket_x, rocket_v = ro.get_ephemerides(t2 - t1, 60)
  
  earth_x = np.array(earth_x)
  mars_x = np.array(mars_x)
  rocket_x = np.array(rocket_x)
  
  fig = go.Figure()
  fig.add_trace(go.Scatter3d(x=earth_x[:,0], y=earth_x[:,1], z=earth_x[:,2], marker_size=1, marker_color='blue'))
  fig.add_trace(go.Scatter3d(x=mars_x[:,0], y=mars_x[:,1], z=mars_x[:,2], marker_size=1, marker_color='orange'))
  fig.add_trace(go.Scatter3d(x=rocket_x[:,0], y=rocket_x[:,1], z=rocket_x[:,2], marker_size=1, marker_color='green'))
  fig.show()
  </code></pre>
  <p>This method generated following plot:</p>
  <p><a href="https://i.stack.imgur.com/WAeb3.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/WAeb3.png" alt="Better orbit" /></a></p>
  <p>Also, before this is mentioned again, I have varied my integration step size and lambert solver tolerance to no avail, the result was qualitatively different.</p>