Lauszus, I made use of your already functional Kalman Filter algorithm in my project: A self stabilizing platform.
You code worked great and you saved me a lot of time 
Thank you
Here are the results:
I also posted the project in the Exhibition forum, in this post.