(particle, z, Q)
| 183 | |
| 184 | |
| 185 | def compute_weight(particle, z, Q): |
| 186 | |
| 187 | lm_id = int(z[2]) |
| 188 | xf = np.array(particle.lm[lm_id, :]).reshape(2, 1) |
| 189 | Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2]) |
| 190 | zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q) |
| 191 | |
| 192 | dz = z[0:2].reshape(2, 1) - zp |
| 193 | dz[1, 0] = pi_2_pi(dz[1, 0]) |
| 194 | |
| 195 | try: |
| 196 | invS = np.linalg.inv(Sf) |
| 197 | except np.linalg.linalg.LinAlgError: |
| 198 | return 1.0 |
| 199 | |
| 200 | num = math.exp(-0.5 * dz.T @ invS @ dz) |
| 201 | den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf)) |
| 202 | |
| 203 | w = num / den |
| 204 | |
| 205 | return w |
| 206 | |
| 207 | |
| 208 | def proposal_sampling(particle, z, Q): |
no test coverage detected