Posición incorrecta durante la propagación de la órbita en Unity

He estado trabajando en un módulo de física orbital durante meses y he llegado a una barrera. Tengo dos métodos, uno para convertir elementos de Kepler a coordenadas planetarias, y el otro para determinar los elementos de Kepler a partir de los vectores de estado R y V. El PositionFromOrbit()método propaga el cuerpo alrededor de la órbita, pero comienza desplazado de su lugar inicial en la órbita. Luego, después de un período, comienza a saltar de un punto a otro en la órbita. Aquí hay un ejemplo:

El cubo azul es la posición actual del objeto antes del tiempo de ejecución, y el contorno rojo es la posición calculada del objeto según sus elementos keplerianos. Puedo ver que el contorno está exactamente encima del apoapsis, pero no sé cómo ni por qué. Aquí están los fragmentos principales, comenzando con la conversión de vectores de estado a elementos keplerianos:

public static KeplerOrbit OrbitFromState(Vector3Decimal R, Vector3Decimal V, decimal mu)
        {
            Vector3Decimal h = Vector3Decimal.Cross(R, V);
            Vector3Decimal n = Vector3Decimal.Cross(Vector3Decimal.forward, h);
            Vector3Decimal eVec = ((V.magnitude * V.magnitude - mu / R.magnitude) * R - Vector3Decimal.Dot(R, V) * V) / mu; // eccentricity vector
            decimal eMag = eVec.magnitude;
            decimal E = V.magnitude * V.magnitude / 2 - mu / R.magnitude;
            decimal a;
            decimal p;
            if(eVec.magnitude != 1)
            {
                a = -mu / (2 * E);
                p = a * (1 - eMag * eMag);
            }
            else
            {
                a = 0;
                p = h.magnitude * h.magnitude / mu;
            }
            decimal inc = ACos(h.z / h.magnitude);

            decimal O;
            if(inc == 0 || inc == Pi)
            {
                O = 0;
            }
            else
            {
                O = ACos(n.x / n.magnitude);
            }

            if(n.y < 0)
            {
                O = 2 * Pi - O;
            }

            decimal w;
            if (eVec.magnitude == 0)
            {
                w = 0;
            }
            else
            {
                if (inc == 0 || inc == Pi)
                {
                    w = ATan2(eVec.y, eVec.x);
                }
                else
                {
                    w = ACos(Vector3Decimal.Dot(n, eVec) / (n.magnitude * eVec.magnitude));
                }
            }
            if(eVec.z < 0)
            {
                w = 2 * Pi - w;
            }

            decimal nu;
            if(eVec.magnitude == 0)
            {
                if(inc == 0 || inc == Pi)
                {
                    nu = ACos(R.z / R.magnitude);
                }
                else
                {
                    nu = ACos(Vector3Decimal.Dot(n, R) / (n.magnitude * R.magnitude));
                }
            }
            else
            {
                nu = ACos(Vector3Decimal.Dot(eVec, R) / (eVec.magnitude * R.magnitude));
                if(Vector3Decimal.Dot(R,V) < 0)
                {
                    nu = 2 * Pi - nu;
                }
            }

            KeplerOrbit ret = new KeplerOrbit(E, a, eVec.magnitude, inc, O, w, nu);
            return ret;

        }

Y aquí está el método para encontrar su posición:

public static Vector3Decimal PositionFromOrbit(KeplerOrbit orbit, decimal t, decimal mu)
        {
            decimal a = orbit.semi_major_axis;
            decimal e = orbit.eccentricity;
            decimal i = orbit.inclination;
            decimal O = orbit.longitude_of_ascent;
            decimal w = orbit.argument_of_periapsis;
            //decimal T = Period(orbit.semi_major_axis, mu);
            decimal t0 = TimeAtEccentricAnomaly(a, e, 0, mu);
            decimal Mt;
            if(t == t0)
            {
                t = t0;
                Mt = 0;
            }
            else
            {
                decimal deltaT = (t - t0);
                Mt = deltaT * Sqrt(mu / (a * a * a));
            }

            decimal E = Mt;
            decimal F = E - e * Sin(E) - Mt;
            int j = 0;
            int maxIter = 30;
            decimal delta = 0.000001m;
            while(Abs(F) > delta && j < maxIter)
            {
                //E = (E - F / (1 - e * Cos(E))) % (Pi * 2);
                E = (E - F / (1 - e * Cos(E))); // this is the line that fixed jittering.
                F = E - e * Sin(E) - Mt;
                j++;
            }

            decimal nu = 2 * ATan2(Sqrt(1 + e) * Sin(E / 2), Sqrt(1 - e) * Cos(E / 2));
            decimal rc = a * (1 - e * Cos(E));

            Vector3Decimal o = new Vector3Decimal(rc * Cos(nu), rc * Sin(nu), 0);
            Vector3Decimal odot = new Vector3Decimal(Sin(E), Sqrt(1 - e * e) * Cos(E), 0);
            odot = odot * (Sqrt(mu * a) / rc);
            decimal rx, ry, rz;
            rx = o.x; ry = o.y; rz = o.z;

            rx = (o.x * (Cos(w) * Cos(O) - Sin(w) * Cos(i) * Sin(O)) - o.y * (Sin(w) * Cos(O) + Cos(w) * Cos(i) * Sin(O)));
            ry = (o.x * (Cos(w) * Sin(O) + Sin(w) * Cos(i) * Cos(O)) + o.y * (Cos(w) * Cos(i) * Cos(O) - Sin(w) * Sin(O)));
            rz = (o.x * (Sin(w) * Sin(i)) + o.y * (Cos(w) * Sin(i)));

            Vector3Decimal r = new Vector3Decimal(rx, ry, rz);
            return r;

        }

¿Alguien puede detectar dónde el yo o los métodos están fallando en su tarea?

Mis vectores iniciales: R = 15, -0.07, 3.36 V = -0.39, 5.14, -0.04

He tenido este problema durante algunas semanas y sería un gran avance si se solucionara. Intenté agregar/restar el argumento de la periapsis de la verdadera anomalía como un novato, pero hizo que la órbita fuera inexacta. Intenté relacionar la posición con el momento del periápside y tampoco funcionó. Adopté otros tres métodos que no funcionaron tan bien como el actual.

¡Gracias a todos!

Editar: Encontré otro síntoma del problema u otro: después de una órbita suave, el objeto comienza a saltar alrededor de su órbita. Viaja del periapsis a la apoapsis y viceversa antes de que comience a temblar.

Editar: El nerviosismo se ha resuelto, gracias a notovny, quien recomendó que lo elimine % (2 * Pi)al resolver la anomalía excéntrica.