I've been there:
and I'd like to say a nice explanation and cool design...do you share your own Kalman filter code and/or other files of your project online? I couldn't find on the boram. I'm just developing a version of IPRobot and it's helpful to see more example (I've gathered a couple of them already).
If possible please let me know where to get your code