The Kalman Filter values from here work, but I think I can make it more responsive. The Lego motor has a optical sensor to track the rotation of the motor, so I have set up a simple program to compare the difference between the angle from the Kalman filter and the angle suggested by the optical sensor.
I really need to upgrade to the latest TI development board simply because it has a hardware quadrature decoder. I not 100% convinced that my software solution does not miss things.
