UKF for basic 3D estimation case now works.
Had a sign error in the measurement error term.