Hi, in 'add_new_lm', do you think Gz should be defined as Jacobian w.r.t the lamdmark's coordinate as follows, instead of the range and bearing?

dx = r * c
dy = r * s
d2 = dx2 + dy2
d = math.sqrt(d2)
Gz = np.array([[dx / d, dy / d],
[-dy / d2, dx / d2]])
particle.lmP[2 * lm_id:2 * lm_id + 2] = np.linalg.inv(Gz) @ Q @ np.linalg.inv(Gz.T)

Thanks!