split headers into cpp files. added high res PF to explore and then subsample
have angle observation function implemented
two robot angle obs setup