working on slam-state
working on making casadi cost correct. close.