Improved error calculation

I noticed the error calculation didn’t seem to behave as expected so I did a little research and found an equation to find the angle between two quaternions and I’m using that angle as an error. I had to write two new functions, one to get the conjugate of a quaternion which was pretty simple and one to get the inverse of a quaternion which wasn’t to hard either.