Wrong position during orbit propagation in Unity

I've been working on an orbital physics module for months now, and have reached a barrier. I have two methods, one to convert from Kepler elements to planet-centric coordinates, and the other to determine the Kepler elements from the state vectors R and V. The PositionFromOrbit() method propagates the body around the orbit, but it starts displaced from it's initial place in the orbit. Then, after one period it starts jumping from point to point on the orbit. Here is an example: The blue cube is the current position of the object before runtime, and the red outline is the calculated position of the object according to it's Keplerian elements. I can see that the outline is exactly on top of the apoapsis, but I don't know how or why. Here's the main snippets, starting with the conversion of state vectors to Keplerian elements:

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;

}

And here's the method to find its position:

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;

}

Can anyone spot where the I or the methods are failing at their task?

My starting vectors: R = 15, -0.07, 3.36 V = -0.39, 5.14, -0.04

I have had this issue for a few weeks now and it would be a great breakthrough if it was fixed. I have tried adding/subtracting the argument of periapsis from true anomaly like a noob but it made the orbit inaccurate. I tried relating the position to the time at periapsis and that also did not work. I adopted three other methods that didn't work as well as the current one.

Thank you, one and all!

Edit: Found another symptom of the issue or another: After a smooth orbit, the object begins to jump around on its orbit. It travels from periapsis to apoapsis and back before it starts to jitter.

Edit: The jittering has been solved, thanks to notovny, who reccommended I remove % (2 * Pi) when solving for the eccentric anomaly.

• How eccentric, inclined is your test orbit? Please provide the parameters used to initialise the test orbit shown in the figure. Are you facing the same problems when testing with a highly inclined, highly eccentric orbit?
– AJN
Nov 7 '21 at 0:45
• The one thing that jumps out at me: If $E$ is Eccentric anomaly, and $w$ is Argument of Periapsis, you should not be adding them together; Eccentric anomaly is completely independent of argument of periapsis. Nov 7 '21 at 0:45
• @AJN The test orbit was very slightly inclined, but I just tested an orbit with an inclination of about 40 degrees and one very close to 90 degrees, and both continued to have the same symptoms with unnoticeable difference. Nov 7 '21 at 1:16
• @notovny oopsie, took out that line and tested it again... no difference, unfortunately. Nov 7 '21 at 1:17
• The other thing that seems odd is that attempting to rectify $E$ to the range $(0,2\pi]$ with the modulus operator inside your Newton's method loop is probably a bad idea, as it takes a smooth function and tosses a periodic discontinuity into it for Newton-Raphson to trip over each time an orbit is completed. I'd probably remove the % (Pi * 2) entirely. Also seconding the request by AJN for a set of parameters that produce the undesirable behavior, as well as links to the reference you used to produce these equations. Nov 7 '21 at 3:09

If you want high fidelity orbit propagation, you should store the orbit in Cartesian form because it's a non-singular orbit representation. It's also the simplest form to account for extra forces on the spacecraft because you only need to integrate $$\vec F = m \vec a$$ and apply it to the Cartesian state without trying to figure out how the given force changes eccentricity, semi-major axis, etc.