Modify finite differences interval for angles and angular velocities
Inovation after k=1000
Fixing innovation by fixing initialisation of input vector
changing time step of finite difference jacobians