Update function updated

Position intergrates using RK2, fixed rotation quaternion.