making initial_trust_box_size smaller gets rid of line search error in forces
casadi slam-state compiles
have a script that modfies a casadi file for our use
working on slam-state
working on making casadi cost correct. close.
slam-state runs. fast, but sometimes reaches max iter or line search fails
slam-state sometimes crashes/doesn't reach goal waypoint. related to penalty coeff (also related to linearization, initial trajectory, and input bounds)
things to try:
- increase time steps
- trust box size
- slam-traj input initializations