Simple Kalman now quite works

TODO: limit Euler angles between +-180 and +-90 degrees