State-Space numerical code