(particle, z, Q)
| 164 | |
| 165 | |
| 166 | def update_landmark(particle, z, Q): |
| 167 | |
| 168 | lm_id = int(z[2]) |
| 169 | xf = np.array(particle.lm[lm_id, :]).reshape(2, 1) |
| 170 | Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2]) |
| 171 | |
| 172 | zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q) |
| 173 | |
| 174 | dz = z[0:2].reshape(2, 1) - zp |
| 175 | dz[1, 0] = pi_2_pi(dz[1, 0]) |
| 176 | |
| 177 | xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf) |
| 178 | |
| 179 | particle.lm[lm_id, :] = xf.T |
| 180 | particle.lmP[2 * lm_id:2 * lm_id + 2, :] = Pf |
| 181 | |
| 182 | return particle |
| 183 | |
| 184 | |
| 185 | def compute_weight(particle, z, Q): |
no test coverage detected