(particle, xf, Pf, Q)
| 128 | |
| 129 | |
| 130 | def compute_jacobians(particle, xf, Pf, Q): |
| 131 | dx = xf[0, 0] - particle.x |
| 132 | dy = xf[1, 0] - particle.y |
| 133 | d2 = dx**2 + dy**2 |
| 134 | d = math.sqrt(d2) |
| 135 | |
| 136 | zp = np.array( |
| 137 | [d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]).reshape(2, 1) |
| 138 | |
| 139 | Hv = np.array([[-dx / d, -dy / d, 0.0], |
| 140 | [dy / d2, -dx / d2, -1.0]]) |
| 141 | |
| 142 | Hf = np.array([[dx / d, dy / d], |
| 143 | [-dy / d2, dx / d2]]) |
| 144 | |
| 145 | Sf = Hf @ Pf @ Hf.T + Q |
| 146 | |
| 147 | return zp, Hv, Hf, Sf |
| 148 | |
| 149 | |
| 150 | def update_KF_with_cholesky(xf, Pf, v, Q, Hf): |
no test coverage detected