(xTrue, xd, u, RFID)
| 306 | |
| 307 | |
| 308 | def observation(xTrue, xd, u, RFID): |
| 309 | |
| 310 | # calc true state |
| 311 | xTrue = motion_model(xTrue, u) |
| 312 | |
| 313 | # add noise to range observation |
| 314 | z = np.zeros((3, 0)) |
| 315 | |
| 316 | for i in range(len(RFID[:, 0])): |
| 317 | |
| 318 | dx = RFID[i, 0] - xTrue[0, 0] |
| 319 | dy = RFID[i, 1] - xTrue[1, 0] |
| 320 | d = math.sqrt(dx**2 + dy**2) |
| 321 | angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) |
| 322 | if d <= MAX_RANGE: |
| 323 | dn = d + np.random.randn() * Qsim[0, 0] # add noise |
| 324 | anglen = angle + np.random.randn() * Qsim[1, 1] # add noise |
| 325 | zi = np.array([dn, pi_2_pi(anglen), i]).reshape(3, 1) |
| 326 | z = np.hstack((z, zi)) |
| 327 | |
| 328 | # add noise to input |
| 329 | ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] |
| 330 | ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + OFFSET_YAWRATE_NOISE |
| 331 | ud = np.array([ud1, ud2]).reshape(2, 1) |
| 332 | |
| 333 | xd = motion_model(xd, ud) |
| 334 | |
| 335 | return xTrue, z, xd, ud |
| 336 | |
| 337 | |
| 338 | def motion_model(x, u): |
no test coverage detected