Hi all,
I have built a NMHE for a UAV to reject disturbances. Acados works really nicely again 
. However, I’m facing the following issue and am curious what you think of it. My odometry gives me a yaw between -pi and pi rad.
In the MPC I solved the issue where I want to go from -pi+SMALLNUMBER to pi by finding the smallest angle between the current and the reference and adding that to the refernce yaw. That works rather nicely.
However, in my MHE, the drone gets unstable when I pass this point. My measurement is -pi at moment A and pi at moment B due to least squares on the difference. I will implement the same strategy as the MPC for now (look at what the last measurement was and add the minimum angle diff), but am looking for a more robust solution in the future.
A more robust solution in the future is in my eyes to find a way to optimize angles in acados, like this small library (incompatible with code generation) tries to achieve: GitHub - ami-iit/liecasadi: Rigid transform using Lie groups and Dual Quaternions, written in CasADi!. It basically comes down to mapping your angles to a manifold where you optimize and then move back. This manifold has nice characteristics making the solver converge faster.
Please let me know if you have encountered similar issues/ wishes for such a feature.
Kind regards,
Rinto