Arduino + LEGO = SEGWAY

This is exactly what I am trying to build. I have started with servos and a Ping))) sensor to measure the distance to the ground, but that is very sensitive to ground being level. I have a dual axis accelerometer and a single axis gyro from sparkfun so I intend to use these for balancing. I am building right now the encoders for the new DC motors I got (100 RPM @ 6V). The only thing I don't quite understand how to implement the Kalman filter. I haven't seen any project like this to have the code available. Perhaps that's why there are only a hanfull of them. If you think you can give me some advice or even better a chance to look at your code, I would be eternaly gratefull.

Here is the tutorial I wrote on the simple balancing robot I have built, based on some code from Parallax forum:
http://www.societyofrobots.com/member_tutorials/node/185
It has the code in Bascom and Arduino.