truncation semi working
tentatively finished obsfunc and cost for planar
working on SQP. need to test cost function and EKF first