Fix UKF Euler angle handling

[ayrton04]

Addresses #777.

The main issue is that UKF weights can be negative. That's fine in general; the UKF only cares whether the weights sum to 1. What we typically see is that the "mean" state has a very large negative weight, and then each of the lambda points gets a small positive weight.

Unfortunately, our use of Euler angles bites us (again) here, because the way we have to do weighted averaging requires us to decompose each Euler angle into x and y components using cos and sin, respectively. We then weight those values, sum them, and then recover the final weighted average via atan2. And therein lies the problem.

atan2 will return the following values (image taken from Wikipedia):

So if the UKF weights end up changing the sign of the summed x component of the Euler angle decomposition, we end up completely changing the recovered angle by +/- pi. That's obviously a huge problem that rears its head whenever the covariance on those values gets too large.

This is another case where a different representation of the angles would be the best way to solve this problem. But I am not going to make changes that fundamental at this stage in this package's life cycle, so I went another route. For angles, I think the signs of the weights can all be positive. All we care about is that the first sample is represented much more heavily than the others. It seems to solve the problem stated in #777, and it also still passes all tests. I've verified the behavior on the test bags visually as well.

[peci1]

So, I've finally got to testing this.

It's definitely better as now at least the UKF node doesn't hang. However, the state estimation still has some problem compared to EKF.

I attach two bags, one running the EKF node, one running the UKF node, the exact same settings. I drove the robot forward, turn right, backward, turn left, backward, forward, backward. These motions are quite ok in the EKF bag, but the UKF estimate is quite bad - backwards motion is sometimes ignored (the first one), or interpreted as forward motion (the other ones).

I haven't played with any of the UKF-specific variables.

I use this config for integrating the 2D odometry:

odom0: husky_velocity_controller/odom
odom0_differential: false
odom0_nodelay: true
odom0_queue_size: 10
odom0_config: [
  false, false, false,   # x_pos, y_pos, z_pos
  false, false, false,   # roll, pitch, yaw
  true, true, true,      # x_vel, y_vel, z_vel 
  false, false, true,    # roll_vel, pitch_vel, yaw_vel
  false, false, false    # x_accel, y_accel, z_accel
]

Looking at the twist parts of the odometry messages, they seem to match the 2D odometry input. So only the position estimate is wrong.

778.zip

[ayrton04]

Thanks for the new data. This looks like a numerical issue with the UKF.

For reference, for a state with dimension N, a UKF creates a set of 2 * N + 1 lambda points. A lambda point is just the state vector with specific dimensions perturbed in a controlled manner (the first point is just the current state, with nothing perturbed). It then projects them through the state transition function, and computes a weighted sum of the projected points. In our case, the state vector is size 15, so we end up with 31 lambda points.

The issue here is the weights themselves. The weights must sum to 1. If we follow the paper's original math for creating the weights, we end up with one very large weight of -999999.0 and 30 weights with value 33333.3333333. This meets the requirement of summing to 1, but ends up creating some bizarre numerical issues.

The original issue when this ticket was created was that the negative weight was affecting the decomposed Euler angles and changing the values we get when we eventually move them back into Euler space via atan2. I solved that, but now we have the following situation.

I'll focus on just the X dimension of the state in this case. The weights are as I described earlier. I'll call the weight vector w:

w =   [999999
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333
          33333.3333333333]

Here are the values for X position after projecting each lambda point through the state transition function. I'll call this vector x:

x =      [-1.79117362289
             -1.7860985556
            -1.79117362681
            -1.79117461965
            -1.79117362289
              -1.791176307
            -1.79117379708
            -1.79116007687
            -1.79117362303
            -1.79117362289
            -1.79117362289
            -1.79117362289
            -1.79117362289
            -1.79117224188
            -1.79117362291
            -1.79117362289
            -1.79624869019
            -1.79117362662
            -1.79117461965
            -1.79117362289
              -1.791176307
            -1.79117379563
            -1.79118716892
             -1.7911736227
            -1.79117362289
            -1.79117362289
            -1.79117362289
            -1.79117362289
             -1.7911750039
            -1.79117362288
            -1.79117362289]

The values are all around -1.79, with some very small pertubations, as expected. But when we compute w' * x, we end up with -2.04838362259761. So we're ending up with a weighted sum that is outside the range of any values in the set of numbers.

There are other strategies for setting the weights. I'll try some simpler ones.