Fixed some errors with RK4 function

Only fixed it where I was using it via FSI, I need to go back and fix it
everywhere