(particles)
| 74 | |
| 75 | |
| 76 | def calc_final_state(particles): |
| 77 | |
| 78 | xEst = np.zeros((STATE_SIZE, 1)) |
| 79 | |
| 80 | particles = normalize_weight(particles) |
| 81 | |
| 82 | for i in range(N_PARTICLE): |
| 83 | xEst[0, 0] += particles[i].w * particles[i].x |
| 84 | xEst[1, 0] += particles[i].w * particles[i].y |
| 85 | xEst[2, 0] += particles[i].w * particles[i].yaw |
| 86 | |
| 87 | xEst[2, 0] = pi_2_pi(xEst[2, 0]) |
| 88 | # print(xEst) |
| 89 | |
| 90 | return xEst |
| 91 | |
| 92 | |
| 93 | def predict_particles(particles, u): |
no test coverage detected