kalmans work

preparation for master Kalman filter
and unscented update for orientation...