about to make slam.cpp to solve linking errors
initial slam-belief works with trajopt initializations