(particle, z, Q)
| 206 | |
| 207 | |
| 208 | def proposal_sampling(particle, z, Q): |
| 209 | |
| 210 | lm_id = int(z[2]) |
| 211 | xf = particle.lm[lm_id, :].reshape(2, 1) |
| 212 | Pf = particle.lmP[2 * lm_id:2 * lm_id + 2] |
| 213 | # State |
| 214 | x = np.array([particle.x, particle.y, particle.yaw]).reshape(3, 1) |
| 215 | P = particle.P |
| 216 | zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q) |
| 217 | |
| 218 | Sfi = np.linalg.inv(Sf) |
| 219 | dz = z[0:2].reshape(2, 1) - zp |
| 220 | dz[1] = pi_2_pi(dz[1]) |
| 221 | |
| 222 | Pi = np.linalg.inv(P) |
| 223 | |
| 224 | particle.P = np.linalg.inv(Hv.T @ Sfi @ Hv + Pi) # proposal covariance |
| 225 | x += particle.P @ Hv.T @ Sfi @ dz # proposal mean |
| 226 | |
| 227 | particle.x = x[0, 0] |
| 228 | particle.y = x[1, 0] |
| 229 | particle.yaw = x[2, 0] |
| 230 | |
| 231 | return particle |
| 232 | |
| 233 | |
| 234 | def update_with_observation(particles, z): |
no test coverage detected