More intuitive way of plot_covariance_ellipse()

[hotsuyuki]

Reference issue

What does this implement/fix?

This change suggests a more intuitive way of plot_covariance_ellipse() in extended_kalman_filter.py and unscented_kalman_filter.py. (This change doesn't affect the result.)

In plot_covariance_ellipse(), np.linalg.eig(Pxy) returns eigval and eigvec like below.

eigval = [ l1, l2 ]

eigvec = [
    [v1_x, v2_x],
    [v1_y, v2_y]
]

So, I think it would be more plausible if the angle of the ellipse and the rot matrix are calculated by

angle = atan2(v1_y, v1_x)

rot = [
    [cos(angle), -sin(angle)],
    [sin(angle),  cos(angle)]
]

Additional information

The result is the same between before and after.

Before:

angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])

rot = np.array([[math.cos(angle), math.sin(angle)],
                [-math.sin(angle), math.cos(angle)]]))

↓

After:

angle_ = math.atan2(eigvec[1, bigind], eigvec[0, bigind])

rot_ = np.array([[math.cos(angle_), -math.sin(angle_)],
                [math.sin(angle_), math.cos(angle_)]]))

The reason why is because the eigenvectors v1 and v2 are orthogonal and the norm is 1.

Therefore, v2_x = -v1_y,

eigvec[bigind, 0] = v1_x
                  = eigvec[0, bigind]

eigvec[bigind, 1] = v2_x
                  = -v1_y
                  = -eigvec[1, bigind]

angle = atan2(eigvec[bigind, 1], eigvec[bigind, 0])
      = atan2(v2_x, v1_x)
      = atan2(-v1_y, v1_x)
      = -atan2(v1_y, v1_x)
      = -atan2(eigvec[1, bigind], eigvec[0, bigind])
      = -angle_

So,

rot(angle) = rot_^T(angle)
           = rot_^T(-angle_)
           = rot_(angle_)

[AtsushiSakai]

Thank you for your PR. I agree the original code is not intuitive. Let's fix it. I have some comments. PTAL.

[AtsushiSakai]

Could you please use scipy Rot?

PythonRobotics/Localization/particle_filter/particle_filter.py

Line 192 in 468be04

rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]

[hotsuyuki]

Ok, I've changed the rotation matrix to scipy.spatial.transform.Rotation.from_euler() in 7a49ca9

[AtsushiSakai]

Thank you for fixing it. This is the last review. PTAL.

[AtsushiSakai]

typo? eigvec -> eig_vec, bigind -> big_ind?

[hotsuyuki]

Sorry, those were typos. I fixed it in a0d1c74.

[AtsushiSakai]

typo? eigvec -> eig_vec, bigind -> big_ind?

[hotsuyuki]

Sorry, those were typos. I fixed it in a0d1c74.

[AtsushiSakai]

LGTM. Thank you so much for your contribution!!