Fixed implementation of RK4

Should be golden now. It's also a little more efficient.