Add simple kalman filter
Fix package.xml