(particles, u)
| 91 | |
| 92 | |
| 93 | def predict_particles(particles, u): |
| 94 | |
| 95 | for i in range(N_PARTICLE): |
| 96 | px = np.zeros((STATE_SIZE, 1)) |
| 97 | px[0, 0] = particles[i].x |
| 98 | px[1, 0] = particles[i].y |
| 99 | px[2, 0] = particles[i].yaw |
| 100 | ud = u + (np.random.randn(1, 2) @ R).T # add noise |
| 101 | px = motion_model(px, ud) |
| 102 | particles[i].x = px[0, 0] |
| 103 | particles[i].y = px[1, 0] |
| 104 | particles[i].yaw = px[2, 0] |
| 105 | |
| 106 | return particles |
| 107 | |
| 108 | |
| 109 | def add_new_lm(particle, z, Q): |
no test coverage detected