SAM working with pinv. crucial bugs were G = -I and wrapping the angle when computing errors. also fixed the motion covariance
now the motion covariance is fixed. it was commented out in the last commit