Code might be wrong: the true trajectory in "particle filter" algorithm shouldn't be a circle

[felixchenfy]

In the current code in Localization/particle_filter/particle_filter.py,
the resultant true trajectory is a circle, as shown below:

(The blue line under the red line is the true trajectory, which is a circle. The figure is copied from README)

However, due to the noise (e.g., wheel slips, noise in the motor), the true trajectory cannot be a perfect circle. So the code might be wrong.

In my view, the "true trajectory" and "dead reckoning trajectory" needs to be swapped inside the function def observation to solve the error.

Besides, for the random noise, I think

np.random.randn() * Rsim[0, 0]

should be replaced by:

np.random.randn() * Rsim[0, 0]**0.5

(Because Rsim is the variance)

[felixchenfy]

Also, in function "def calc_covariance",
the covariance should be divided by the number of samples.

The current code is:

def calc_covariance(xEst, px, pw):
    cov = np.zeros((3, 3))

    for i in range(px.shape[1]):
        dx = (px[:, i] - xEst)[0:3]
        cov += pw[0, i] * dx.dot(dx.T)

    return cov

[AtsushiSakai]

@felixchenfy Hi. Thank you for pointing out!!. I appreciate your help. You are pointing out 3 problems for Particle filter simulation.

[AtsushiSakai]

1) However, due to the noise (e.g., wheel slips, noise in the motor), the true trajectory cannot be a perfect circle. So the code might be wrong. I think It depends on prerequisites. In this simulation, if the robot can move on a perfect circle, but the gyro and wheel odometry sensor has noise. I want to do simulation in the ideal situation. But you are right in real situation, there might be a slip and error of control input, so it is difficult to move on a perfect circle.

[AtsushiSakai]

2. Covariance noise problem, 3. Covariance value calculation problem. I think you are right. I tried to fix it. Could you please check this PR? #235

[felixchenfy]

@AtsushiSakai Hi, I checked the code and then ran the program, and it's good!
Thank you for fixing it. And also thank you for providing these very helpful robotics codes!

[felixchenfy]

1. However, due to the noise (e.g., wheel slips, noise in the motor), the true trajectory cannot be a perfect circle. So the code might be wrong. I think It depends on prerequisites. In this simulation, if the robot can move on a perfect circle, but the gyro and wheel odometry sensor has noise. I want to do simulation in the ideal situation. But you are right in real situation, there might be a slip and error of control input, so it is difficult to move on a perfect circle.

I think there is still something not correct.

You replied to me that the gyro and wheel odometry sensor has noise. If this is the cause that deviates the dead reckoning result from the perfect circle, then this noise R_sim is supposed to be added to the state variables angle and displacement in the form of x = F.dot(x) + B.dot(u) + noise. However, in lines 57~59, I see that the noise R_sim is added to the control input (which I understand as that the motor has some error).

ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
ud = np.array([[ud1, ud2]]).T

Just want to make sure if I understand it correctly.

Thanks again for discussing with me. @AtsushiSakai

[felixchenfy]

@AtsushiSakai Also, In my understanding, R_sim is the noise that makes the state variables different from the ideal motion model's result. If R_sim is non-zero, the state variables should contain noise -- in this case, not a perfect circle.

You mentioned the gyro and wheel odometry sensor has noise. Since these are sensor nose, they are supposed to be added to the observation model, instead of the motion model. (My previous reply says they need to be added to x = F.dot(x) + B.dot(u) + noise, but I think I was wrong. Now I feel it needs to be added to the observation model)

[AtsushiSakai]

@felixchenfy

this noise R_sim is supposed to be added to the state variables angle and displacement in the form of x = F.dot(x) + B.dot(u) + noise. However, in lines 57~59, I see that the noise R_sim is added to the control input (which I understand as that the motor has some error).

I think it is depends on how do you model the noise. In this simulation, ud1 is a wheel velocity measurement with noise, ud2 is a gyro yawrate measurement with noise.

[felixchenfy]

@AtsushiSakai OK. I see. Thank you!

[AtsushiSakai]

@felixchenfy Thank you for your kind report. If you find any other problems, feel free to add an issue and PR : ).

[regexident]

FYI: I pointed out (and fixed) the same issue for the KF/EKF implementation in PR https://github.com/AtsushiSakai/PythonRobotics/pull/191, which was closed without an actual fix. As such you might want to revisit that PR. :)

[felixchenfy]

@regexident I think AtsushiSakai is assuming there is no control noise, but only observation noise on three sensors: ①lidar, ②gyroscope, and ③wheel encoder.
The lidar noise is modeled into the observation model,
and the gyroscope and encoder noise is modelled into the motion model here.

I agree with him that the logic itself makes sense, but it's just being different than the standard PF (or KF) formula and cannot be applied to the real world, because in the real world there is control noise.

[regexident]

@felixchenfy Agreed.

I just wanted to make sure @AtsushiSakai is aware of it, so that PF and KF/EKF stay in sync, as they’re shown next to each other for a supposedly identical scenario, which would be even more confusing otherwise. :)

[malleshamdasari]

Hey all (@atsushi-sakai @felixchenfy @regexident ), thanks for the insights. I am new to these topics. Can you please point me to a tutorial where I can understand this PF implementation? Thank you.

[regexident]

@mdasari823 The Kalman-and-Bayesian-Filters-in-Python project has a nice chapter on Particle Filters (PF) that should get you started. Just keep in mind that, as discussed above, the way noise is modeled here might look upside down compared to most other PF implementations/tutorials.

[malleshamdasari]

@regexident Thank you so much for the link. That is exactly what I was looking for.