Trying to fix SARSE
Added target state body-frame velocity. Analysis suggests we should have observability, but do not.
Not sure where the problem is, if this can be made to work.
Check derivation of EQOM, but I think they're right.