2-body propagation

[jhnrs]

hey guys,

I'm trying to make program/game in unity that involves orbital mechanics. I would like to get the physics more or less simular to KSP's rail system. So I found this paper where i got most of my equations from and

video which explains orbital propagation. But when I try to implement it ,the orbiting body/satellite slows down at some point in its orbit and just keeps oscilating back and forth around that point. When I play it backwards (negative timeScale value) it speeds up and slow down at roughly the same point in it's orbit. This is my code (C#):

using UnityEngine;using System.Collections;


public class GameController {


    public GameObject satellite;
    public float timeScale;


    void Start()
    {
        orbit = new Orbit (4f, 0.6f, 0.0f, 1.0f, 0.0f, 0.0f); //orbital parameters
    }


    void Update()
    {
        satellite.transform.Position = orbit.NextPosition (Time.deltaTime*timeScale);
    }
}

using UnityEngine;
using System.Collections;


public class Orbit {
    public float a;        //Semi-major axis
    public float e;        //eccentricity
    public float i;        //inclination
    public float w;        //argument of perigee
    public float LAN;    //longitude of the ascending node
    public float MA;    //mean anomaly
    public float n;        //mean motion


    public float TA;    //True anomaly
    public float EA;    //eccentric anomaly


    public float Mu = 1f;


    Vector3 P;
    Vector3 Q;



    public Orbit(float semiMajorAxis,float eccentricity,float inclination,float argPerigee,float longAscNode,float trAn,GameObject orbitPrimary)
    {
        primary = orbitPrimary;
        a = semiMajorAxis;
        e = eccentricity;
        i = inclination;
        w = argPerigee;
        LAN = longAscNode;


        float EA = TAtoEA (trAn);


        MA = EAtoMA (EA);




        n = Mathf.Sqrt (Mu / (a * a * a));


        SetPQ ();
    }




    void SetPQ()
    {
        P = (Mathf.Cos(w)*Mathf.Cos(LAN)-Mathf.Sin(w)*Mathf.Cos(i)*Mathf.Sin(LAN))*Vector3.right+
            (Mathf.Sin(w)*Mathf.Sin(i))*Vector3.up+
            (Mathf.Cos(w)*Mathf.Sin(LAN)+Mathf.Sin(w)*Mathf.Cos(i)*Mathf.Cos(LAN))*Vector3.forward;



        Q = (-Mathf.Sin(w)*Mathf.Cos(LAN)-Mathf.Cos(w)*Mathf.Cos(i)*Mathf.Sin(LAN))*Vector3.right+
            (Mathf.Sin(i)*Mathf.Cos(w))*Vector3.up+
            (-Mathf.Sin(w)*Mathf.Sin(LAN)+Mathf.Cos(w)*Mathf.Cos(i)*Mathf.Cos(LAN))*Vector3.forward;


    }




    public Vector3 NextPosition(float passedTime)
    {


        MA = EAtoMA (EA);
        MA = MA + n * passedTime;


        if(MA > 2*Mathf.PI)
            MA -= 2*Mathf.PI;
        if(MA < 0)
            MA += 2*Mathf.PI;


        EA = MAtoEA (MA);
        TA = EAtoTA (EA);

        return a*(Mathf.Cos(EA)-e)*P+a*Mathf.Sqrt(1-e*e)*Mathf.Sin(EA)*Q;


    }




    float TAtoEA(float T)
    {
        float E =  Mathf.Acos ((e+Mathf.Cos(T))/(1+e*Mathf.Cos(T)));
        if (T > Mathf.PI && T < 2 * Mathf.PI)
            E = 2 * Mathf.PI - E;

        return E;
    }



    float EAtoMA(float E)
    {
        return E - e * Mathf.Sin (E); 
    }




    float MAtoEA(float M)
    {
        float E = M;
        float Enext = E-(E-e*Mathf.Sin(E)-M)/(1-e*Mathf.Cos(E));;

        while (Enext-E>Mathf.Pow (10,-15)) 
        {
            E = Enext;
            Enext = E-(E-e*Mathf.Sin(E)-M)/(1-e*Mathf.Cos(E));
        }


        return Enext;
    }



    float EAtoTA(float E)
    {
        float T = Mathf.Acos ((Mathf.Cos(E)-e)/(1-e*Mathf.Cos(E)));
        if (E > Mathf.PI && E < 2* Mathf.PI)
            T = 2 * Mathf.PI - T;
        return T;
    }


}

If one of you could find my mistake it would be really helpfull and if you have a question about my code ,please ask?

ps. if you have any sugestions or know a better (please not to complicated) way to calculate the position of a satellite ,please tell me