Work on localization

- test with fixed poses
- still not working properly