Kalman filter implementation: 2D linear system Unscented 2D system with bearing Unscented 3d -> 2D linear system projection TODO: Extended Kalman filter implementation