kalman prepared for orientation

- made code a lot prettier
- moved working version that estimates position to \apps
- prepared state vector with quaternion and angular velocity