def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ x = eta[0:3] ## reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T numM = m.shape[1] Rot = rotmat2d(-x[2]) delta_m = m - x[:2, None] zc = delta_m - Rot @ self.sensor_offset[:, None] zpred = self.h(eta).reshape(-1, 2).T zr = zpred[0] Rpihalf = rotmat2d(np.pi / 2) # In what follows you can be clever and avoid making this for all the landmarks you _know_ # you will not detect (the maximum range should be available from the data). # But keep it simple to begin with. # Allocate H and set submatrices as memory views into H # You may or may not want to do this like this H = np.zeros( (2 * numM, 3 + 2 * numM)) # TODO, see eq (11.15), (11.16), (11.17) Hx = H[:, :3] # slice view, setting elements of Hx will set H as well Hm = H[:, 3:] # slice view, setting elements of Hm will set H as well # proposed way is to go through landmarks one by one jac_z_cb = -np.eye( 2, 3) # preallocate and update this for some speed gain if looping for i in range(numM): # But this hole loop can be vectorized ind = 2 * i # starting postion of the ith landmark into H inds = slice(ind, ind + 2) # the inds slice for the ith landmark into H jac_z_cb[:, 2] = -Rpihalf @ delta_m[:, i] Hx[ind] = (zc[:, i] / zr[i]) @ jac_z_cb Hx[ind + 1] = (zc[:, i] / zr[i]**2) @ Rpihalf.T @ jac_z_cb Hm[inds, inds] = -Hx[inds, :2] assert ( H.shape == (2 * numM, 3 + 2 * numM) ), "EKFSLAM.H: Wrong shape on calculated H" # TODO: You can set some assertions here to make sure that some of the structure in H is correct return H
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T numM = m.shape[1] Rot = rotmat2d(x[2]) Rpihalf = rotmat2d(np.pi / 2) # Relative position of landmark to robot in world frame: delta_m = m - x[:2].reshape((2, 1)) zc = ( delta_m.T - Rot @ self.sensor_offset ).T # (2, #measurements), each measured position in cartesian coordinates like # [x coordinates; # y coordinates] H = np.zeros((2 * numM, 3 + 2 * numM)) z = np.zeros((numM, 1)) ones = np.ones((numM, 1)) Hx_tops = -np.concatenate( ((1 / la.norm(zc, axis=0) * delta_m).T, z), axis=1) Hx_bottoms = np.concatenate( ((1 / la.norm(zc, axis=0)**2 * delta_m).T @ Rpihalf, -ones), axis=1) Hx = np.concatenate((Hx_tops, Hx_bottoms), axis=1) Hx = Hx.reshape(-1, 3) Hm = -Hx[:, :2] Hm = Hm.reshape(numM, -1, 2) H[:, :3] = Hx H[:, 3:] = la.block_diag(*Hm) return H
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ # extract states and map x = eta[:3] # reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T numM = m.shape[1] Rot = rotmat2d(-x[2]) delta_m = m - x[:2, None] zc = delta_m - Rot @ self.sensor_offset[:, None] zpred = self.h(eta).reshape(-1, 2).T # predicted measurements zr = zpred[0] # ranges # Allocate H and set submatrices as memory views into H H = np.zeros((2 * numM, 3 + 2 * numM)) Hx = H[:, :3] Hm = H[:, 3:] Rpihalf = rotmat2d(np.pi / 2) jac_z_cb = -np.eye(2, 3) #jac_z_cb[:, 2] = -Rpihalf @ delta_m # [-I_2 -Rph(m^i - rho_k)] for i in range(numM): # But this whole loop can be vectorized """if zr[i] > self.max_range: Hx[ind] = zc[:, i]# @ jac_z_cb / zr[i] Hx[ind + 1] = zc[:, i]# @ Rpihalf.T @ jac_z_cb / zr[i]**2 Hm[inds, inds] = -Hx[inds, :2] continue""" ind = 2 * i # starting position of the ith landmark into H inds = slice(ind, ind + 2) # the inds slice for the ith landmark into H jac_z_cb[:, 2] = -Rpihalf @ delta_m[:, i] # > do this before loop Hx[ind] = zc[:, i] @ jac_z_cb / zr[i] Hx[ind + 1] = zc[:, i] @ Rpihalf.T @ jac_z_cb / zr[i]**2 Hm[inds, inds] = -Hx[inds, :2] assert (H.shape == ( 2 * numM, 3 + 2 * numM)), "EKFSLAM.H: Wrong shape on calculated H" return H
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ x = eta[0:3] ## reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T numM = m.shape[1] Rot = rotmat2d(-x[2]) delta_m = m - x[:2, None] zc = delta_m - Rot @ self.sensor_offset[:, None] zpred = self.h(eta).reshape(-1, 2).T zr = zpred[0] Rpihalf = rotmat2d(np.pi / 2) # Allocate H and set submatrices as memory views into H H = np.zeros( (2 * numM, 3 + 2 * numM)) # TODO, see eq (11.15), (11.16), (11.17) Hx = H[:, :3] # slice view, setting elements of Hx will set H as well Hm = H[:, 3:] # slice view, setting elements of Hm will set H as well jac_z_cb = -np.eye(2, 3) for i in range(numM): ind = 2 * i # starting postion of the ith landmark into H inds = slice(ind, ind + 2) # the inds slice for the ith landmark into H jac_z_cb[:, 2] = -Rpihalf @ delta_m[:, i] Hx[ind] = (zc[:, i] / zr[i]) @ jac_z_cb Hx[ind + 1] = (zc[:, i] / zr[i]**2) @ Rpihalf.T @ jac_z_cb Hm[inds, inds] = -Hx[inds, :2] assert ( H.shape == (2 * numM, 3 + 2 * numM) ), "EKFSLAM.H: Wrong shape on calculated H" # TODO: You can set some assertions here to make sure that some of the structure in H is correct return H
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[:, j] is the jth landmark m = eta[3:].reshape((-1, 2)).T Rot = rotmat2d(-x[2]) # None as index ads an axis with size 1 at that position. # Numpy broadcasts size 1 dimensions to any size when needed # TODO, relative position of landmark to sensor on robot in world frame # mi − ρk − R(ψk)L. Where ρk = x[:2], mi = m, R(ψk) = Rot, L = self.sensor_offset delta_m = m - x[:2].reshape( 2, 1 ) - rotmat2d(x[2]) @ (self.sensor_offset).reshape( 2, 1 ) # TODO, relative position of landmark to sensor on robot in world frame # TODO, predicted measurements in cartesian coordinates, beware sensor offset for VP zpredcart = np.array([Rot @ delta_mi for delta_mi in delta_m.T]) # TODO, ranges zpred_r = np.array([la.norm(mi) for mi in delta_m.T]) # TODO, bearings zpred_theta = np.array([np.arctan2(mi[1], mi[0]) for mi in zpredcart]) # TODO, the two arrays above stacked on top of each other vertically like zpred = np.vstack([zpred_r, zpred_theta]) # [ranges; # bearings] # into shape (2, #lmrk) zpred = zpred.T.ravel( ) # stack measurements along one dimension, [range1 bearing1 range2 bearing2 ...] assert (zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3), "SLAM.h: Wrong shape on zpred" return zpred
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ #print('h') # extract states and map x = eta[:3] ## reshape map (2, #landmarks), m[:, j] is the jth landmark m = eta[3:].reshape((-1, 2)).T Rot = rotmat2d(-x[2]) # relative position of landmark to sensor on robot in world frame delta_m = m - x[:2, None] - Rot.T @ self.sensor_offset[:, None] # predicted measurements in cartesian coordinates zpredcart = Rot @ delta_m zpred_r = la.norm(zpredcart, axis=0) # ranges zpred_theta = np.arctan2(zpredcart[1], zpredcart[0]) # bearings zpred = np.stack((zpred_r, zpred_theta)) zpred = zpred.T.ravel() # stack measurements along one dimension assert (zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3), "SLAM.h: Wrong shape on zpred" return zpred
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters eta : (3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ x = eta[:3] rho = x[:2] m = eta[3:].reshape((-1, 2)).T R = rotmat2d(-x[2]) # world->body # R.T: body->world # relative position of landmark to sensor on robot (world) delta_m = m - rho.reshape(2, 1) # No sensor offset here # predicted measurements in cartesian coordinates zpredcart = R @ delta_m - self.sensor_offset.reshape(2, 1) # Converting cartesian body -> polar body zpred_r = np.apply_along_axis(np.linalg.norm, axis=0, arr=zpredcart) zpred_theta = utils.wrapToPi(np.arctan(zpredcart[1] / zpredcart[0])) zpred = np.concatenate((zpred_r, zpred_theta), axis=0) # stack measurements along one dimension, [range1 bearing1 range2 bearing2 ...] zpred = zpred.T.ravel() assert (zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3), "SLAM.h: Wrong shape on zpred" return zpred
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[:, j] is the jth landmark m = eta[3:].reshape((-1, 2)).T Rot = rotmat2d(-x[2]) # None as index ads an axis with size 1 at that position. # Numpy broadcasts size 1 dimensions to any size when needed delta_m = m - (x[:2, None] + Rot.T @ self.sensor_offset[:, None]) zpredcart = Rot @ delta_m zpred_r = np.linalg.norm(zpredcart, axis=0) z_pred_theta = np.arctan2(zpredcart[1], zpredcart[0]) zpred = np.vstack((zpred_r, z_pred_theta)) zpred = zpred.T.ravel( ) # stack measurements along one dimension, [range1 bearing1 range2 bearing2 ...] assert (zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3), "SLAM.h: Wrong shape on zpred" return zpred
def f(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: """Add the odometry u to the robot state x. Parameters ---------- x : np.ndarray, shape=(3,) the robot state u : np.ndarray, shape=(3,) the odometry Returns ------- np.ndarray, shape = (3,) the predicted state """ """ psi_prev = x[2] psi_prev = utils.wrapToPi(psi_prev) x_pred = np.zeros((3,)) x_pred[0] = x[0] + u[0]*np.cos(psi_prev)-u[1]*np.sin(psi_prev) # TODO, eq (11.7). Should wrap heading angle between (-pi, pi), see utils.wrapToPi x_pred[1] = x[1] + u[0]*np.sin(psi_prev) + u[1]*np.cos(psi_prev) x_pred[2] += u[2] x_pred[2] = utils.wrapToPi(x_pred[2]) """ x_pred = np.hstack( [x[0:2] + rotmat2d(x[2]) @ u[:2], utils.wrapToPi(x[2] + u[2])]) assert x_pred.shape == (3, ), "EKFSLAM.f: wrong shape for xpred" return x_pred
def f(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: """Add the odometry u to the robot state x. Parameters ---------- x : np.ndarray, shape=(3,) the robot state u : np.ndarray, shape=(3,) the odometry Returns ------- np.ndarray, shape = (3,) the predicted state """ # xpred = np.zeros(3,) # xpred[0] = x[0] + u[0]*np.cos(x[2]) - u[1]*np.sin(x[2]) # xpred[1] = x[1] + u[0]*np.sin(x[2]) + u[1]*np.cos(x[2]) # xpred[2] = utils.wrapToPi(x[2] + u[2]) #eq (11.7). Should wrap heading angle between (-pi, pi), see utils.wrapToPi pos = x[:2] + rotmat2d(x[2]) @ u[:2] yaw_angle = utils.wrapToPi(x[2] + u[2]) xpred = np.array([*pos, yaw_angle]) assert xpred.shape == (3, ), "EKFSLAM.f: wrong shape for xpred" return xpred
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters: eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns: np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ x = eta[:3] m = eta[3:].reshape( (-1, 2)).T ## reshape map (2, #landmarks), m[j] is the jth landmark numM = m.shape[1] R = rotmat2d(x[2]) # body->world Rpihalf = rotmat2d(np.pi / 2) # relative position of landmark to robot in world frame. m - rho that appears in (11.15) and (11.16) delta_m = m - x[:2].reshape(2, 1) # (world) zc = delta_m - R @ self.sensor_offset.reshape( 2, 1) # measurements, cartesian (world) zr = np.apply_along_axis(np.linalg.norm, axis=0, arr=zc) Hx_top = np.concatenate( (-(1 / la.norm(zc, axis=0) * zc).T, np.zeros((numM, 1))), axis=1) Hx_btm = np.concatenate( ((1 / (la.norm(zc, axis=0)**2) * zc).T @ Rpihalf, -np.ones( (numM, 1))), axis=1) Hx = np.concatenate((Hx_top, Hx_btm), axis=1).reshape(-1, 3) Hm = -Hx[:, :2].reshape(numM, -1, 2) H = np.zeros((2 * numM, 3 + 2 * numM)) H[:, :3] = Hx H[:, 3:] = la.block_diag(*Hm) #assert (H.shape[0] == 2*numM and H.shape[1] == 3+2*numM), "SLAM.H: Wrong shape of H" #assert (Hx.shape[0] == 2*numM and Hx.shape[1] == 3), "SLAM.H: Wrong shape of Hx" return H
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[:, j] is the jth landmark m = eta[3:].reshape((-1, 2)).T Rot = rotmat2d(-x[2]) # Rot_neg = rotmat2d(-x[2]) # sverre: WORLD -> BODY # Rot_pos = rotmat2d(x[2]) # sverre: BODY -> WORLD # None as index ads an axis with size 1 at that position. # Numpy broadcasts size 1 dimensions to any size when needed # delta_m = np.array(list(map(lambda l: l - x[:2] - Rot.T @ self.sensor_offset, m.T))) # TODO, relative position of landmark to sensor on robot in world frame # zpredcart = np.array(list(map(lambda d_m: Rot @ d_m, delta_m))) # TODO, predicted measurements in cartesian coordinates, beware sensor offset for VP # zpred_r = np.array(list(map(lambda rel_pos: la.norm(rel_pos), delta_m))) # TODO, ranges # zpred_theta = np.array(list(map(lambda rel_pos: np.arctan2(rel_pos[0], rel_pos[1]), zpredcart))) # TODO, bearings # zpred = np.stack((zpred_r, zpred_theta)) # TODO, the two arrays above stacked on top of each other vertically like # [ranges; # bearings] # into shape (2, #lmrk) delta_m = m - (x[:2, None] + Rot.T @ self.sensor_offset[:, None]) zpredcart = Rot @ delta_m zpred_r = np.linalg.norm(zpredcart, axis=0) z_pred_theta = np.arctan2(zpredcart[1], zpredcart[0]) zpred = np.vstack((zpred_r, z_pred_theta)) zpred = zpred.T.ravel( ) # stack measurements along one dimension, [range1 bearing1 range2 bearing2 ...] # assert ( # zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3 # ), "SLAM.h: Wrong shape on zpred" return zpred
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[:, j] is the jth landmark m = eta[3:].reshape((-1, 2)).T M = m.shape[1] Rot = rotmat2d(-x[2]) # None as index ads an axis with size 1 at that position. # Numpy broadcasts size 1 dimensions to any size when needed delta_m = (m.T - x[:2]).T # m = (2, 1000) # x = (2,) zpredcart = Rot @ delta_m - self.sensor_offset.reshape((2, 1)) # (2x2) @ (2,1000) zpred_r = la.norm(zpredcart, axis=0) zpred_theta = np.arctan2(zpredcart[1], zpredcart[0]) zpred = np.vstack([zpred_r, zpred_theta]) assert len(zpred_r) == M assert len(zpred_theta) == M # vertically like # [ranges; # bearings] # into shape (2, #lmrk) zpred = zpred.T.ravel( ) # stack measurements along one dimension, [range1 bearing1 range2 bearing2 ...] assert (zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3), "SLAM.h: Wrong shape on zpred" return zpred
def h(self, eta: np.ndarray) -> np.ndarray: """Predict all the landmark positions in sensor frame. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks,) The landmarks in the sensor frame. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[:, j] is the jth landmark m = eta[3:].reshape((-1, 2)).T Rot = rotmat2d(-x[2]) # None as index ads an axis with size 1 at that position. # Numpy broadcasts size 1 dimensions to any size when needed # TODO, relative position of landmark to sensor on robot in world frame delta_m = (m.T - x[:2]) # TODO, predicted measurements in cartesian coordinates, beware sensor offset for VP zpredcart = delta_m - Rot.T @ self.sensor_offset z_body = Rot @ zpredcart.T zpred_r = la.norm(zpredcart, axis=1) # TODO, ranges zpred_theta = np.arctan2(z_body[1], z_body[0]) # TODO, bearings zpred = np.stack( (zpred_r, zpred_theta) ) # TODO, the two arrays above stacked on top of each other vertically like # [ranges; # bearings] # into shape (2, #lmrk) zpred = zpred.T.ravel( ) # stack measurements along one dimension, [range1 bearing1 range2 bearing2 ...] assert (zpred.ndim == 1 and zpred.shape[0] == eta.shape[0] - 3), "SLAM.h: Wrong shape on zpred" return zpred
def Fu(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: """Calculate the Jacobian of f with respect to u. Parameters ---------- x : np.ndarray, shape=(3,) the robot state u : np.ndarray, shape=(3,) the odometry Returns ------- np.ndarray The Jacobian of f wrt. u. """ Fu = block_diag(rotmat2d(x[2]), 1) assert Fu.shape == (3, 3), "EKFSLAM.Fu: wrong shape" return Fu
def Fx(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: """Calculate the Jacobian of f with respect to x. Parameters ---------- x : np.ndarray, shape=(3,) the robot state u : np.ndarray, shape=(3,) the odometry Returns ------- np.ndarray The Jacobian of f wrt. x. """ psi = x[2] Fx = np.eye(3) # TODO, eq (11.13) Fx[0:2, 2] = rotmat2d(psi + np.pi / 2) @ u[:2] assert Fx.shape == (3, 3), "EKFSLAM.Fx: wrong shape" return Fx
def f(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: """Add the odometry u to the robot state x. Parameters ---------- x : np.ndarray, shape=(3,) the robot state u : np.ndarray, shape=(3,) the odometry Returns ------- np.ndarray, shape = (3,) the predicted state """ pos = x[:2] + rotmat2d(x[2]) @ u[:2] yaw_angle = utils.wrapToPi(x[2] + u[2]) # Wrap heading angle between (-pi, pi) xpred = np.array([*pos, yaw_angle]) assert xpred.shape == (3,), "EKFSLAM.f: wrong shape for xpred" return xpred
def Fu(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: """Calculate the Jacobian of f with respect to u. Parameters ---------- x : np.ndarray, shape=(3,) the robot state u : np.ndarray, shape=(3,) the odometry Returns ------- np.ndarray The Jacobian of f wrt. u. """ psi = x[2] Fu = np.eye(3) # TODO, eq (11.14) Fu[0:2, 0:2] = rotmat2d(psi) assert Fu.shape == (3, 3), "EKFSLAM.Fu: wrong shape" return Fu
def add_landmarks(self, eta: np.ndarray, P: np.ndarray, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """Calculate new landmarks, their covariances and add them to the state. Parameters ---------- eta : np.ndarray, shape=(3 + 2*#landmarks,) the robot state and map concatenated P : np.ndarray, shape=(3 + 2*#landmarks,)*2 the covariance of eta z : np.ndarray, shape(2 * #newlandmarks,) A set of measurements to create landmarks for Returns ------- Tuple[np.ndarray, np.ndarray], shapes=(3 + 2*(#landmarks + #newlandmarks,), (3 + 2*(#landmarks + #newlandmarks,)*2 eta with new landmarks appended, and its covariance """ n = P.shape[0] assert z.ndim == 1, "SLAM.add_landmarks: z must be a 1d array" numLmk = z.shape[0] // 2 lmnew = np.empty_like(z) Gx = np.empty((numLmk * 2, 3)) Rall = np.zeros((numLmk * 2, numLmk * 2)) sensor_offset_world = rotmat2d( eta[2] ) @ self.sensor_offset # For transforming landmark position into world frame sensor_offset_world_der = rotmat2d( eta[2] + np.pi / 2) @ self.sensor_offset # Used in Gx rot = rotmat2d(z[1::2] + eta[2]) #new_rot = rot.swapaxes(0,2).swapaxes(1,2).ravel().reshape((18,2)) vecZ = np.array([-np.sin(z[1::2] + eta[2]), np.cos(z[1::2] + eta[2])]).T Gx[:, :2] = np.tile(np.eye(2), (numLmk, 1)) Gx[:, 2] = (z[0::2].reshape(-1, 1) * vecZ + sensor_offset_world_der).ravel() lmnew = ((z[0::2] * rot[:, 0]).T + eta[0:2] + sensor_offset_world).ravel() right = np.insert(z[0::2], np.arange(0, numLmk, 1), 0) left = np.tile([1, 0], numLmk) np.concatenate((left, right)).reshape(-1, 2, order="F") for j in range(numLmk): ind = 2 * j inds = slice(ind, ind + 2) zj = z[inds] Gz = rot[:, :, j] @ np.diag([1, zj[0]]) # TODO Rall[ inds, inds] = Gz @ self.R @ Gz.T # TODO, Gz * R * Gz^T, transform measurement covariance from polar to cartesian coordinates assert len(lmnew) % 2 == 0, "SLAM.add_landmark: lmnew not even length" etaadded = np.hstack( (eta, lmnew)) # TODO, append new landmarks to state vector n_R = Rall.shape[0] Padded = np.zeros((n + n_R, n + n_R)) Padded[:n, :n] = P Padded[n:, n:] = Gx @ P[:3, :3] @ Gx.T + Rall Padded[n:, :n] = Gx @ P[:3, :] Padded[:n, n:] = Padded[n:, :n].T assert ( etaadded.shape * 2 == Padded.shape ), "EKFSLAM.add_landmarks: calculated eta and P has wrong shape" assert np.allclose( Padded, Padded.T), "EKFSLAM.add_landmarks: Padded not symmetric" assert np.all(np.linalg.eigvals(Padded) >= 0 ), "EKFSLAM.add_landmarks: Padded not PSD" return etaadded, Padded
def add_landmarks(self, eta: np.ndarray, P: np.ndarray, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """Calculate new landmarks, their covariances and add them to the state. Parameters ---------- eta : np.ndarray, shape=(3 + 2*#landmarks,) the robot state and map concatenated P : np.ndarray, shape=(3 + 2*#landmarks,)*2 the covariance of eta z : np.ndarray, shape(2 * #newlandmarks,) A set of measurements to create landmarks for Returns ------- Tuple[np.ndarray, np.ndarray], shapes=(3 + 2*(#landmarks + #newlandmarks,), (3 + 2*(#landmarks + #newlandmarks,)*2 eta with new landmarks appended, and its covariance """ #print('add_landmark') assert z.ndim == 1, "SLAM.add_landmarks: z must be a 1d array" n = P.shape[0] numLmk = z.shape[0] // 2 lmnew = np.empty_like(z) Gx = np.empty((numLmk * 2, 3)) Rall = np.zeros((numLmk * 2, numLmk * 2)) I2 = np.eye(2) # Preallocate, used for Gx # For transforming landmark position into world frame sensor_offset_world = rotmat2d(eta[2]) @ self.sensor_offset sensor_offset_world_der = rotmat2d(eta[2] + np.pi / 2) @ self.sensor_offset # For Gx for j in range(numLmk): ind = 2 * j inds = slice(ind, ind + 2) zj = z[inds] rot = rotmat2d(zj[1] + eta[2]) # psi + psi # calculate position of new landmark in world frame lmnew[inds] = zj[0] * rot[:, 0] + eta[:2] + sensor_offset_world Gx[inds, :2] = I2 Gx[inds, 2] = zj[ 0] * rot[:, 1] + sensor_offset_world_der #.reshape(-1, 1) #+ sensor_offset_world_der Gz = rot @ np.diag([1, zj[0]]) Rall[inds, inds] = Gz @ self.R @ Gz.T # pol2cart on measurement cov assert len(lmnew) % 2 == 0, "SLAM.add_landmark: lmnew not even length" # append new landmarks to state vector #etaadded = np.append(eta, lmnew) etaadded = np.array([*eta, *lmnew]) Padded = la.block_diag(P, Gx @ P[:3, :3] @ Gx.T + Rall) Padded[n:, :n] = Gx @ P[:3, :] Padded[:n, n:] = Padded[n:, :n].T assert ( etaadded.shape * 2 == Padded.shape ), "EKFSLAM.add_landmarks: calculated eta and P has wrong shape" assert np.allclose( Padded, Padded.T), "EKFSLAM.add_landmarks: Padded not symmetric" assert np.all(np.linalg.eigvals(Padded) >= 0 ), "EKFSLAM.add_landmarks: Padded not PSD" return etaadded, Padded
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T numM = m.shape[1] Rot = rotmat2d(x[2]) Rpihalf = rotmat2d(np.pi / 2) delta_m = m - x[:2].reshape( (2, 1) ) # TODO, relative position of landmark to robot in world frame. m - rho that appears in (11.15) and (11.16) zc = ( delta_m.T - Rot @ self.sensor_offset ).T # TODO, (2, #measurements), each measured position in cartesian coordinates like # [x coordinates; # y coordinates] # NOTE: Calculate zpred here for some speed increase probably zpred = self.h( eta) # TODO (2, #measurements), predicted measurements, like zpred = np.reshape(zpred, (2, zpred.shape[0] // 2), "F") # [ranges; # bearings] #zr = zpred[0, :] # TODO, ranges # Allocate H and set submatrices as memory views into H # You may or may not want to do this like this H = np.zeros( (2 * numM, 3 + 2 * numM)) # TODO, see eq (11.15), (11.16), (11.17) z = np.zeros((numM, 1)) ones = np.ones((numM, 1)) Hx_tops = -np.concatenate( ((1 / la.norm(zc, axis=0) * delta_m).T, z), axis=1) # Correct Hx_bottoms = np.concatenate( ((1 / la.norm(zc, axis=0)**2 * delta_m).T @ Rpihalf, -ones), axis=1) # Correct Hx = np.concatenate((Hx_tops, Hx_bottoms), axis=1) Hx = Hx.reshape(-1, 3) Hm = -Hx[:, :2] Hm = Hm.reshape(numM, -1, 2) H[:, :3] = Hx H[:, 3:] = la.block_diag(*Hm) # TODO: You can set some assertions here to make sure that some of the structure in H is correct return H
num_asso = np.count_nonzero(a[mk] > -1) if num_asso > 0: NISnorm[mk] = NIS[mk] / (2 * num_asso) CInorm[mk] = np.array(chi2.interval(confidence_prob, 2 * num_asso)) / (2 * num_asso ) else: NISnorm[mk] = 1 CInorm[mk].fill(1) xupd[mk] = eta[:3] if doPlot: sh_lmk.set_offsets(eta[3:].reshape(-1, 2)) if len(z) > 0: zinmap = ( rotmat2d(eta[2]) @ ( z[:, 0] * np.array([np.cos(z[:, 1]), np.sin(z[:, 1])]) + slam.sensor_offset[:, None] ) + eta[0:2, None] ) sh_Z.set_offsets(zinmap.T) lh_pose.set_data(*xupd[mk_first:mk, :2].T) ax.set( xlim=[-200, 200], ylim=[-200, 200], title=f"step {k}, laser scan {mk}, landmarks {len(eta[3:])//2},\nmeasurements {z.shape[0]}, num new = {np.sum(a[mk] == -1)}", ) plt.draw()
num_asso = np.count_nonzero(a[mk] > -1) if num_asso > 0: NISnorm[mk] = NIS[mk] / (2 * num_asso) CInorm[mk] = np.array(chi2.interval(confidence_prob, 2 * num_asso)) / (2 * num_asso) else: NISnorm[mk] = 1 CInorm[mk].fill(1) xupd[mk] = eta[:3] if doPlot: sh_lmk.set_offsets(eta[3:].reshape(-1, 2)) if len(z) > 0: zinmap = (rotmat2d(eta[2]) @ (z[:, 0] * np.array( [np.cos(z[:, 1]), np.sin(z[:, 1])]) + slam.sensor_offset[:, None]) + eta[0:2, None]) sh_Z.set_offsets(zinmap.T) lh_pose.set_data(*xupd[mk_first:mk, :2].T) ax.set( xlim=[-200, 200], ylim=[-200, 200], title= f"step {k}, laser scan {mk}, landmarks {len(eta[3:])//2},\nmeasurements {z.shape[0]}, num new = {np.sum(a[mk] == -1)}", ) plt.draw() plt.pause(0.00001)
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters: eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns: np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ x = eta[:3] m = eta[3:].reshape( (-1, 2)).T ## reshape map (2, #landmarks), m[j] is the jth landmark numM = m.shape[1] R = rotmat2d(x[2]) # body->world # relative position of landmark to robot in world frame. m - rho that appears in (11.15) and (11.16) delta_m = m - x[:2].reshape(2, 1) # (world) zc = delta_m - R @ self.sensor_offset.reshape( 2, 1) # measurements, cartesian coord (world frame) zpred = self.h(eta) zr = np.array([zpred[i] for i in range(0, len(zpred), 2)]) # ranges Rpihalf = rotmat2d(np.pi / 2) # In what follows you can be clever and avoid making this for all the landmarks you _know_ # you will not detect (the maximum range should be available from the data). # But keep it simple to begin with. # Allocate H and set submatrices as memory views into H H = np.zeros( (2 * numM, 3 + 2 * numM)) # see eq (11.15), (11.16), (11.17) Hx = H[:, : 3] # slice view, setting elements of THIS will set H as well Hm = H[:, 3:] # proposed way is to go through landmarks one by one jac_z_cb = -np.eye( 2, 3) # preallocate and update this for some speed gain if looping for j in range(numM): # But this whole loop can be vectorized ind = 2 * j # starting postion of the ith landmark into H inds = slice(ind, ind + 2) # the inds slice for the ith landmark into H zc_j = zc[:, j].reshape(2, 1) temp = np.concatenate( (-np.eye(2), -Rpihalf @ delta_m[:, j].reshape(2, 1)), axis=1) Hx_top = (zc_j.T / zr[j]) @ temp Hx_btm = (zc_j.T @ Rpihalf.T / zr[j]**2) @ temp Hx[inds] = np.concatenate((Hx_top, Hx_btm), axis=0) #TODO : avoid making this for all the landmarks you _know_ # you will not detect (the maximum range should be available from the data). Hm[inds, inds] = np.vstack( (zc_j.T / zr[j], zc_j.T @ Rpihalf / (zr[j]**2))) #Hm[inds, inds] = np.vstack(delta_m[:,j].T * 1 / la.norm(delta_m[:,j]), # delta_m[:,j].T @ Rpihalf * 1 / (la.norm(delta_m[:,j])**2) ) #set some assertions here to make sure that some of the structure in H is correct assert (H.shape[0] == 2 * numM and H.shape[1] == 3 + 2 * numM), "SLAM.H: Wrong shape of H" assert (Hx.shape[0] == 2 * numM and Hx.shape[1] == 3), "SLAM.H: Wrong shape of Hx" return H
def add_landmarks(self, eta: np.ndarray, P: np.ndarray, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """Calculate new landmarks, their covariances and add them to the state. Parameters eta : np.ndarray, shape=(3 + 2*#landmarks,) : the robot state and map concatenated P : np.ndarray, shape=(3 + 2*#landmarks,)*2 : the covariance of eta z : np.ndarray, shape(2 * #newlandmarks,) : A set of measurements to create landmarks for Returns Tuple[np.ndarray, np.ndarray], shapes=(3 + 2*(#landmarks + #newlandmarks,), (3 + 2*(#landmarks + #newlandmarks,)*2 eta with new landmarks appended, and its covariance """ n = P.shape[0] assert z.ndim == 1, "SLAM.add_landmarks: z must be a 1d array" numLmk = z.shape[0] // 2 lmnew = np.empty_like(z) Gx = np.empty((numLmk * 2, 3)) Rall = np.zeros((numLmk * 2, numLmk * 2)) I2 = np.eye(2) # Preallocate, used for Gx sensor_offset_world = rotmat2d( eta[2] ) @ self.sensor_offset # For transforming landmark position into world frame sensor_offset_world_der = rotmat2d( eta[2] + np.pi / 2) @ self.sensor_offset # Used in Gx for j in range(numLmk): ind = 2 * j inds = slice(ind, ind + 2) zj = z[inds] # calculate pos of new landmark in world frame zj_x, zj_y = utils.polar2cartesian(zj[0], zj[1]) lmnew[inds] = rotmat2d(eta[2]) @ np.array([zj_x, zj_y]).reshape( 2, ) - sensor_offset_world Gx[inds, :2] = I2 Gx[inds, 2] = zj[0] * np.array([ -np.sin(zj[1] + eta[2]), np.cos(zj[1] + eta[2]) ]).reshape(2, ) + sensor_offset_world_der Gz = rotmat2d(zj[1] + eta[2]) @ np.diag((1, zj[0])) # Gz * R * Gz^T, transform measurement covariance from polar to cartesian coordinates Rall[inds, inds] = Gz @ self.R @ Gz.T assert len(lmnew) % 2 == 0, "SLAM.add_landmark: lmnew not even length" etaadded = np.append( eta, lmnew) # [eta; G() ] : append new landmarks to state vector low_r = Gx @ P[:3, :3] @ Gx.T + Rall # + Gz @ self.R @ Gz.T P_added = la.block_diag(P, low_r) # block diagonal of P_new P_added[:n, n:] = P[:, :3] @ Gx.T # top right corner of P_new P_added[n:, :n] = P_added[:n, n:].T # transpose of above assert ( etaadded.shape * 2 == P_added.shape ), "EKFSLAM.add_landmarks: calculated eta and P has wrong shape" assert np.allclose( P_added, P_added.T), "EKFSLAM.add_landmarks: P_added not symmetric" assert np.all(np.linalg.eigvals(P_added) >= 0 ), "EKFSLAM.add_landmarks: P_added not PSD" return etaadded, P_added
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T numM = m.shape[1] Rot = rotmat2d(x[2]) delta_m = (m.T - x[:2]).T zc = delta_m - (Rot @ self.sensor_offset).reshape((2, 1)) # (2, #measurements), each measured position in cartesian coordinates like # [x coordinates; # y coordinates] zpred = self.h(eta).reshape((-1, 2)).T # (2, #measurements), predicted measurements, like # [ranges; # bearings] zr = zpred[0] Rpihalf = rotmat2d(np.pi / 2) # In what follows you can be clever and avoid making this for all the landmarks you _know_ # you will not detect (the maximum range should be available from the data). # But keep it simple to begin with. # Allocate H and set submatrices as memory views into H # You may or may not want to do this like this H = np.zeros( (2 * numM, 3 + 2 * numM)) # TODO, see eq (11.15), (11.16), (11.17) Hx = H[:, :3] # slice view, setting elements of Hx will set H as well Hm = H[:, 3:] # slice view, setting elements of Hm will set H as well # proposed way is to go through landmarks one by one jac_z_cb = -np.eye( 2, 3) # preallocate and update this for some speed gain if looping for i in range(numM): # But this whole loop can be vectorized ind = 2 * i # starting postion of the ith landmark into H inds = slice(ind, ind + 2) # the inds slice for the ith landmark into H zc_norm = la.norm(zc[:, i]) zc_unit = zc[:, i] / zc_norm jac_z_cb[:, 2] = -Rpihalf @ delta_m[:, i] # TODO: Set H or Hx and Hm here Hx[inds] = np.vstack([zc_unit.T, zc_unit.T @ Rpihalf.T / zc_norm ]) @ jac_z_cb Hm[inds, inds] = np.vstack([zc_unit.T, zc_unit.T / zc_norm @ Rpihalf.T]) # TODO: You can set some assertions here to make sure that some of the structure in H is correct return H
CI[mk] = chi2.interval(1-alpha, 2 * num_asso) total_num_asso += num_asso NISes[mk] = NIS[mk] else: NISnorm[mk] = 1 CInorm[mk].fill(1) CI[mk].fill(1) xupd[mk] = eta[:3] if doPlot: sh_lmk.set_offsets(eta[3:].reshape(-1, 2)) if len(z) > 0: zinmap = ( rotmat2d(eta[2]) @ ( z[:, 0] * np.array([np.cos(z[:, 1]), np.sin(z[:, 1])]) + slam.sensor_offset[:, None] ) + eta[0:2, None] ) sh_Z.set_offsets(zinmap.T) lh_pose.set_data(*xupd[mk_first:mk, :2].T) ax.set( xlim=[-200, 200], ylim=[-200, 200], title=f"step {k}, laser scan {mk}, landmarks {len(eta[3:])//2},\nmeasurements {z.shape[0]}, num new = {np.sum(a[mk] == -1)}", ) plt.draw()
def add_landmarks(self, eta: np.ndarray, P: np.ndarray, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """Calculate new landmarks, their covariances and add them to the state. Parameters ---------- eta : np.ndarray, shape=(3 + 2*#landmarks,) the robot state and map concatenated P : np.ndarray, shape=(3 + 2*#landmarks,)*2 the covariance of eta z : np.ndarray, shape(2 * #newlandmarks,) A set of measurements to create landmarks for Returns ------- Tuple[np.ndarray, np.ndarray], shapes=(3 + 2*(#landmarks + #newlandmarks,), (3 + 2*(#landmarks + #newlandmarks,)*2 eta with new landmarks appended, and its covariance """ n = P.shape[0] assert z.ndim == 1, "SLAM.add_landmarks: z must be a 1d array" numLmk = z.shape[0] // 2 lmnew = np.empty_like(z) x = eta[:3] Gx = np.empty((numLmk * 2, 3)) Rall = np.zeros((numLmk * 2, numLmk * 2)) I2 = np.eye(2) # Preallocate, used for Gx sensor_offset_world = rotmat2d( eta[2] ) @ self.sensor_offset # For transforming landmark position into world frame sensor_offset_world_der = rotmat2d( eta[2] + np.pi / 2) @ self.sensor_offset # Used in Gx for j in range(numLmk): ind = 2 * j inds = slice(ind, ind + 2) zj = z[inds] zr, zb = zj rot = rotmat2d(zb + x[2]) zc = zr * np.array([np.cos(zb + x[2]), np.sin(zb + x[2])]) lmnew[inds] = x[:2] + zc + sensor_offset_world # TODO, calculate position of new landmark in world frame Gx[inds, :2] = I2 Gx[inds, 2] = zr * np.array([ -np.sin(zb + x[2]), np.cos(zb + x[2]) ]) + sensor_offset_world_der Gz = rot @ np.diag([1, zr]) Rall[inds, inds] = Gz @ self.R @ Gz.T # TODO, Gz * R * Gz^T, transform measurement covariance from polar to cartesian coordinates assert len(lmnew) % 2 == 0, "SLAM.add_landmark: lmnew not even length" etaadded = np.array([*eta, *lmnew]) Padded = block_diag(P, Gx @ P[:3, :3] @ Gx.T + Rall) # TODO, block diagonal of P_new, see problem text in 1g) in graded assignment 3 Padded[:n, n:] = P[:, :3] @ Gx.T Padded[n:, :n] = Padded[:n, n:].T assert ( etaadded.shape * 2 == Padded.shape ), "EKFSLAM.add_landmarks: calculated eta and P has wrong shape" assert np.allclose( Padded, Padded.T), "EKFSLAM.add_landmarks: Padded not symmetric" assert np.all(np.linalg.eigvals(Padded) >= 0 ), "EKFSLAM.add_landmarks: Padded not PSD" return etaadded, Padded
def add_landmarks(self, eta: np.ndarray, P: np.ndarray, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """Calculate new landmarks, their covariances and add them to the state. Parameters ---------- eta : np.ndarray, shape=(3 + 2*#landmarks,) the robot state and map concatenated P : np.ndarray, shape=(3 + 2*#landmarks,)*2 the covariance of eta z : np.ndarray, shape(2 * #newlandmarks,) A set of measurements to create landmarks for Returns ------- Tuple[np.ndarray, np.ndarray], shapes=(3 + 2*(#landmarks + #newlandmarks,), (3 + 2*(#landmarks + #newlandmarks,)*2 eta with new landmarks appended, and its covariance """ n = P.shape[0] assert z.ndim == 1, "SLAM.add_landmarks: z must be a 1d array" numLmk = z.shape[0] // 2 lmnew = np.empty_like(z) Gx = np.empty((numLmk * 2, 3)) Rall = np.zeros((numLmk * 2, numLmk * 2)) I2 = np.eye(2) # Preallocate, used for Gx sensor_offset_world = rotmat2d( eta[2] ) @ self.sensor_offset # For transforming landmark position into world frame sensor_offset_world_der = rotmat2d( eta[2] + np.pi / 2) @ self.sensor_offset # Used in Gx for j in range(numLmk): ind = 2 * j inds = slice(ind, ind + 2) zj = z[inds] zj_cart = zj[0] * np.array([np.cos(zj[1]), np.sin(zj[1])]) rot = rotmat2d(zj[1] + eta[2]) # TODO, rotmat in Gz lmnew[inds] = eta[:2] + rotmat2d( eta[2] ) @ zj_cart + sensor_offset_world # TODO, calculate position of new landmark in world frame Gx[inds, :2] = I2 # TODO Gx[inds, 2] = zj[0] * rot[:, 1] + sensor_offset_world_der # TODO Gz = rot @ np.diag([1, zj[0]]) # TODO Rall[ inds, inds] = Gz @ self.R @ Gz.T # TODO, Gz * R * Gz^T, transform measurement covariance from polar to cartesian coordinates assert len(lmnew) % 2 == 0, "SLAM.add_landmark: lmnew not even length" # TODO, append new landmarks to state vector etaadded = np.append(eta, lmnew) # TODO, block diagonal of P_new, see problem text in 1g) in graded assignment 3 Padded = la.block_diag( P, Gx @ P[:3, :3] @ Gx.T + Rall) #? Ikke transposed på den siste Gx her, ref øvingsteksten # TODO, top right corner of P_new Padded[:n, n:] = P[:, :3] @ Gx.T #? Har endra index her for å få top right # TODO, transpose of above. Should yield the same as calcualion, but this enforces symmetry and should be cheaper Padded[n:, :n] = Padded[:n, n:].T assert ( etaadded.shape * 2 == Padded.shape ), "EKFSLAM.add_landmarks: calculated eta and P has wrong shape" assert np.allclose( Padded, Padded.T), "EKFSLAM.add_landmarks: Padded not symmetric" assert np.all(np.linalg.eigvals(Padded) >= 0 ), "EKFSLAM.add_landmarks: Padded not PSD" return etaadded, Padded
def H(self, eta: np.ndarray) -> np.ndarray: """Calculate the jacobian of h. Parameters ---------- eta : np.ndarray, shape=(3 + 2 * #landmarks,) The robot state and landmarks stacked. Returns ------- np.ndarray, shape=(2 * #landmarks, 3 + 2 * #landmarks) the jacobian of h wrt. eta. """ # extract states and map x = eta[0:3] ## reshape map (2, #landmarks), m[j] is the jth landmark m = eta[3:].reshape((-1, 2)).T #? Copy paste fra h(.) numM = m.shape[1] Rot = rotmat2d(x[2]) # TODO, relative position of landmark to robot in world frame. m - rho that appears in (11.15) and (11.16) delta_m = m - x[:2].reshape(2, 1) # TODO, (2, #measurements), each measured position in cartesian coordinates like zc = delta_m - Rot @ self.sensor_offset.reshape((2, 1)) # zc = [Rot @ mi for mi in delta_m.T] # [x coordinates; # y coordinates] # TODO (2, #measurements), predicted measurements, like zpred = self.h(eta) # [ranges; # bearings] # TODO, ranges zr = zpred[::2] Rpihalf = rotmat2d(np.pi / 2) # In what follows you can be clever and avoid making this for all the landmarks you _know_ # you will not detect (the maximum range should be available from the data). # But keep it simple to begin with. # Allocate H and set submatrices as memory views into H # You may or may not want to do this like this H = np.zeros( (2 * numM, 3 + 2 * numM)) # TODO, see eq (11.15), (11.16), (11.17) Hx = H[:, :3] # slice view, setting elements of Hx will set H as well Hm = H[:, 3:] # slice view, setting elements of Hm will set H as well # proposed way is to go through landmarks one by one jac_z_cb = -np.eye( 2, 3) # preallocate and update this for some speed gain if looping I2 = np.eye(2) for i in range(numM): # But this hole loop can be vectorized ind = 2 * i # starting postion of the ith landmark into H inds = slice(ind, ind + 2) # the inds slice for the ith landmark into H jac_z_cb[:, 2] = -Rpihalf @ delta_m[:, i] Hx[inds, :][0, :] = (zc[:, i].T / zr[i]) @ jac_z_cb Hx[inds, :][1, :] = (zc[:, i].T @ Rpihalf.T / (zr[i]**2)) @ jac_z_cb Hm[inds, inds] = -Hx[inds, :2] # TODO: You can set some assertions here to make sure that some of the structure in H is correct return H