LQR angle wrapping?

Technical Programming
1

Hey, one of my students is working on applying LQR to the rotational component of robot pose, and he’s not sure how to address angle wrapping. Has anyone else successfully applied LQR to theta control?

2

Your model is nonlinear, and will need to linearize.

Exactly how you linearize (e.g. around what point and how often) is important. You can also set up your model to be easier to work with—for example, if you have a differential drivetrain and you use cross track error instead of global x and y error, then you only ever need to linearize around \theta = 0.

Sections 8.7 and 8.9 from Tyler’s book are exactly what you need for a differential drive, but should be educational for other situations as well.

1 Like
3

thanks! I’m not sure this is a linearization issue. imagine a simple one joint arm that can turn multiple times, with an absolute encoder. I’m really just looking for code examples that work in this context.

4

Often, one cares about the error in angle. For something like steering on swerve, given an actual angle and a desired angle, there are two errors – one in each direction of rotation. One of these errors will always go through 0/360, wrapping. In this case, you can always compute both errors (one will always be positive, the other always negative) and take the error with the smaller absolute value.

If the thing you are trying to control cannot go all the way around (e.g. a typical arm joint), it’s often easiest to set your coordinate system so that the wrapping point is aligned with the prohibited region for rotation. This avoids any issue with wrapping.

5

yeah, thanks. i think there’s also an issue with the observation itself, isn’t there? like the kalman filter correct() and predict() steps also need to be aware of the wrapping, don’t they?

6

Yep. The KalmanFilter class doesn’t support modifying the residual calculation, but the ExtendedKalmanFilter class does.

1 Like
7

ahhh, that may be the clue i was looking for, i didn’t look there. thanks! i’ll poke at it some more. if you have an example of using the EKF class for this angle-wrapping scenario (e.g. in a unit test?), that would really be helpful!

8

We use it for the UKF tests, but not the EKF tests. The interface is similar for the EKF though.

9

ok, i made an example of angle-wrapping EKF+LQR:

it’s quite verbose but very simple. i’d be happy to make a version for wpilib if you want this sort of thing.

1 Like