low variance re-sampling
(particles)
| 252 | |
| 253 | |
| 254 | def resampling(particles): |
| 255 | """ |
| 256 | low variance re-sampling |
| 257 | """ |
| 258 | |
| 259 | particles = normalize_weight(particles) |
| 260 | |
| 261 | pw = [] |
| 262 | for i in range(N_PARTICLE): |
| 263 | pw.append(particles[i].w) |
| 264 | |
| 265 | pw = np.array(pw) |
| 266 | |
| 267 | Neff = 1.0 / (pw @ pw.T) # Effective particle number |
| 268 | # print(Neff) |
| 269 | |
| 270 | if Neff < NTH: # resampling |
| 271 | wcum = np.cumsum(pw) |
| 272 | base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE |
| 273 | resampleid = base + np.random.rand(base.shape[0]) / N_PARTICLE |
| 274 | |
| 275 | inds = [] |
| 276 | ind = 0 |
| 277 | for ip in range(N_PARTICLE): |
| 278 | while ((ind < wcum.shape[0] - 1) and (resampleid[ip] > wcum[ind])): |
| 279 | ind += 1 |
| 280 | inds.append(ind) |
| 281 | |
| 282 | tparticles = particles[:] |
| 283 | for i in range(len(inds)): |
| 284 | particles[i].x = tparticles[inds[i]].x |
| 285 | particles[i].y = tparticles[inds[i]].y |
| 286 | particles[i].yaw = tparticles[inds[i]].yaw |
| 287 | particles[i].lm = tparticles[inds[i]].lm[:, :] |
| 288 | particles[i].lmP = tparticles[inds[i]].lmP[:, :] |
| 289 | particles[i].w = 1.0 / N_PARTICLE |
| 290 | |
| 291 | return particles |
| 292 | |
| 293 | |
| 294 | def calc_input(time): |
no test coverage detected