Angle wrapping issues for MHE

MPC Discussions
1

Hi all,

I have built a NMHE for a UAV to reject disturbances. Acados works really nicely again :slight_smile:. 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

2

Hi,

a similar issue is discussed here: Discontinuous angle results in QP solver error - #6 by FreyJo
I think it definitely makes sense to use more advanced angle representations.
Is there any acados feature really missing for doing this?

Best,
Jonathan

3

An alternate option for something like vehicle yaw, is to generate your trajectory yaw setpoints so that between each shooting node or setpoint, the angle doesn’t differ by more than M_PI.

If it does then there is a shorter angle to use and you should add this shorter angle to the yaw setpoint before it, also don’t honour the (M_PI, -M_PI) limits if this means the angle difference is going to end up greater than M_PI.

This will enable acados to optimise and put non-linearity handling in the trajectory generation part of your code (outside of the optimisation part)