def Aerr( # sverre: error state transition matrix self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, ) -> np.ndarray: """Calculate the continuous time error state dynamics Jacobian. Args: x_nominal (np.ndarray): The nominal state, shape (16,) acceleration (np.ndarray): The estimated acceleration for the prediction interval, shape (3,) omega (np.ndarray): The estimated rotation rate for the prediction interval, shape (3,) Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: Continuous time error state dynamics Jacobian, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.Aerr: x_nominal shape incorrect {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.Aerr: acceleration shape incorrect {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.Aerr: omega shape incorrect {omega.shape}" # Rotation matrix R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) # Allocate the matrix A = np.zeros((15, 15)) # Set submatrices A[POS_IDX * VEL_IDX] = np.eye(3) A[VEL_IDX * ERR_ATT_IDX] = -R @ cross_product_matrix(acceleration) A[VEL_IDX * ERR_ACC_BIAS_IDX] = -R A[ERR_ATT_IDX * ERR_ATT_IDX] = -cross_product_matrix( omega ) #tror vi holder oss til omega i BODY her (på samme måte som for quaternion_prediction over) A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = -np.eye(3) A[ERR_ACC_BIAS_IDX * ERR_ACC_BIAS_IDX] = -self.p_acc * np.eye( 3 ) #p_acc er en proporsjonalitetskonstant (tuning-parameter) som bestermmer raten til biaset A[ERR_GYRO_BIAS_IDX * ERR_GYRO_BIAS_IDX] = -self.p_gyro * np.eye(3) #det samme gjelder her # Bias correction A[VEL_IDX * ERR_ACC_BIAS_IDX] = A[VEL_IDX * ERR_ACC_BIAS_IDX] @ self.S_a A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = (A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] @ self.S_g) assert A.shape == ( 15, 15, ), f"ESKF.Aerr: A-error matrix shape incorrect {A.shape}" return A
def Aerr( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, ) -> np.ndarray: """Calculate the continuous time error state dynamics Jacobian. Args: x_nominal (np.ndarray): The nominal state, shape (16,) acceleration (np.ndarray): The estimated acceleration for the prediction interval, shape (3,) omega (np.ndarray): The estimated rotation rate for the prediction interval, shape (3,) Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: Continuous time error state dynamics Jacobian, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.Aerr: x_nominal shape incorrect {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.Aerr: acceleration shape incorrect {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.Aerr: omega shape incorrect {omega.shape}" # Rotation matrix R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) # Allocate the matrix A = np.zeros((15, 15)) # Set submatrices A[POS_IDX * VEL_IDX] = np.eye(3) A[VEL_IDX * ERR_ATT_IDX] = -R @ cross_product_matrix(acceleration) A[VEL_IDX * ERR_ACC_BIAS_IDX] = -R A[ERR_ATT_IDX * ERR_ATT_IDX] = -cross_product_matrix(omega) A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = -np.eye(3) A[ERR_ACC_BIAS_IDX * ERR_ACC_BIAS_IDX] = -(self.p_acc) * np.eye(3) A[ERR_GYRO_BIAS_IDX * ERR_GYRO_BIAS_IDX] = -(self.p_gyro) * np.eye(3) # Bias correction A[VEL_IDX * ERR_ACC_BIAS_IDX] = A[VEL_IDX * ERR_ACC_BIAS_IDX] @ self.S_a A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] @ self.S_g assert A.shape == ( 15, 15, ), f"ESKF.Aerr: A-error matrix shape incorrect {A.shape}" return A
def quaternion_to_rotation_matrix(quaternion: Quaternion): epsilon = quaternion.a eta = [quaternion.b, quaternion.c, quaternion.d] epsilon_cross_matrix = utils.cross_product_matrix(epsilon) R = (np.eye(3) + 2 * eta * epsilon_cross_matrix + 2 * epsilon_cross_matrix @ epsilon_cross_matrix) return R
def inject( self, x_nominal: np.ndarray, delta_x: np.ndarray, P: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray]: """Inject a calculated error state into the nominal state and compensate in the covariance. Args: x_nominal (np.ndarray): The nominal state to inject the error state deviation into, shape (16,) delta_x (np.ndarray): The error state deviation, shape (15,) P (np.ndarray): The error state covariance matrix Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Injected Tuple(x_injected, P_injected): x_injected: The injected nominal state, shape (16,) P_injected: The injected error state covariance matrix, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.inject: x_nominal shape incorrect {x_nominal.shape}" assert delta_x.shape == ( 15, ), f"ESKF.inject: delta_x shape incorrect {delta_x.shape}" assert P.shape == (15, 15), f"ESKF.inject: P shape incorrect {P.shape}" ### Useful concatenation of indices # All injection indices, minus the attitude INJ_IDX = POS_IDX + VEL_IDX + ACC_BIAS_IDX + GYRO_BIAS_IDX # All error indices, minus the attitude DTX_IDX = POS_IDX + VEL_IDX + ERR_ACC_BIAS_IDX + ERR_GYRO_BIAS_IDX x_injected = x_nominal.copy() #: Inject error state into nominal state, except attitude x_injected[INJ_IDX] = x_nominal[INJ_IDX] + delta_x[DTX_IDX] #: Inject attitude q_nominal = x_nominal[ATT_IDX] q_delta = np.array([1, *(0.5 * delta_x[ERR_ATT_IDX])]).reshape(4, ) q_injected = quaternion_product(q_nominal, q_delta) x_injected[ATT_IDX] = q_injected / np.linalg.norm(q_injected) #: Compensate for injection in the covariances G_injected = np.eye(15) G_injected[6:9, 6:9] = np.eye(3) - cross_product_matrix( 0.5 * delta_x[ERR_ATT_IDX]) P_injected = G_injected @ P @ G_injected.T assert x_injected.shape == ( 16, ), f"ESKF.inject: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.inject: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected
def innovation_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Calculates the innovation and its covariance for a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,) P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S): v: innovation, shape (3,) S: innovation covariance, shape (3, 3) """ assert x_nominal.shape == ( 16, ), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}" H = np.eye(3, 15) v = z_GNSS_position - x_nominal[POS_IDX] # TODO: innovation # leverarm compensation if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) v -= R @ lever_arm S = H @ P @ H.T + R_GNSS # TODO: innovation covariance assert v.shape == ( 3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}" assert S.shape == ( 3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}" return v, S
def inject( self, x_nominal: np.ndarray, delta_x: np.ndarray, P: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray]: """Inject a calculated error state into the nominal state and compensate in the covariance. Args: x_nominal (np.ndarray): The nominal state to inject the error state deviation into, shape (16,) delta_x (np.ndarray): The error state deviation, shape (15,) P (np.ndarray): The error state covariance matrix Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Injected Tuple(x_injected, P_injected): x_injected: The injected nominal state, shape (16,) P_injected: The injected error state covariance matrix, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.inject: x_nominal shape incorrect {x_nominal.shape}" assert delta_x.shape == ( 15, ), f"ESKF.inject: delta_x shape incorrect {delta_x.shape}" assert P.shape == (15, 15), f"ESKF.inject: P shape incorrect {P.shape}" ### Useful concatenation of indices # All injection indices, minus the attitude INJ_IDX = POS_IDX + VEL_IDX + ACC_BIAS_IDX + GYRO_BIAS_IDX # All error indices, minus the attitude DTX_IDX = POS_IDX + VEL_IDX + ERR_ACC_BIAS_IDX + ERR_GYRO_BIAS_IDX x_injected = x_nominal.copy() x_injected[INJ_IDX] += delta_x[DTX_IDX] quat_injected = quaternion_product( x_nominal[ATT_IDX], np.block([1, delta_x[ERR_ATT_IDX] / 2])) x_injected[ATT_IDX] = quat_injected / (la.norm(quat_injected, 2)) # Covariance G_injected = la.block_diag( np.eye(6), np.eye(3) - cross_product_matrix(delta_x[ERR_ATT_IDX] / 2), np.eye(6)) P_injected = G_injected @ P @ G_injected.T assert x_injected.shape == ( 16, ), f"ESKF.inject: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.inject: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected
def quaternion_to_rotation_matrix(quaternion: np.ndarray, debug: bool = True) -> np.ndarray: """Convert a quaternion to a rotation matrix Args: quaternion (np.ndarray): Quaternion of either shape (3,) (pure quaternion) or (4,) debug (bool, optional): Debug flag, could speed up by setting to False. Defaults to True. Raises: RuntimeError: Quaternion is of the wrong shape AssertionError: Debug assert fails, rotation matrix is not element of SO(3) Returns: np.ndarray: Rotation matrix of shape (3, 3) """ if quaternion.shape == (4, ): eta = quaternion[0] epsilon = quaternion[1:] elif quaternion.shape == (3, ): eta = 0 epsilon = quaternion.copy() else: raise RuntimeError( f"quaternion.quaternion_to_rotation_matrix: Quaternion to multiplication error, quaternion shape incorrect: {quaternion.shape}" ) R = np.eye(3) + 2 * eta * utils.cross_product_matrix( epsilon) + 2 * utils.cross_product_matrix( epsilon) @ utils.cross_product_matrix(epsilon) if debug: assert np.allclose( np.linalg.det(R), 1 ), f"quaternion.quaternion_to_rotation_matrix: Determinant of rotation matrix not close to 1" assert np.allclose( R.T, np.linalg.inv(R) ), f"quaternion.quaternion_to_rotation_matrix: Transpose of rotation matrix not close to inverse" return R
def quaternion_product(ql: np.ndarray, qr: np.ndarray) -> np.ndarray: """Perform quaternion product according to either (10.21) or (10.34). Args: ql (np.ndarray): Left quaternion of the product of either shape (3,) (pure quaternion) or (4,) qr (np.ndarray): Right quaternion of the product of either shape (3,) (pure quaternion) or (4,) Raises: RuntimeError: Left or right quaternion are of the wrong shape AssertionError: Resulting quaternion is of wrong shape Returns: np.ndarray: Quaternion product of ql and qr of shape (4,)s """ if ql.shape == (4, ): eta_left = ql[0] eps_left = ql[1:].reshape((3, 1)) elif ql.shape == (3, ): eta_left = 0 eps_left = ql.reshape((3, 1)) else: raise RuntimeError( f"utils.quaternion_product: Quaternion multiplication error, left quaternion shape incorrect: {ql.shape}" ) if qr.shape == (4, ): q_right = qr.copy() eta_right = q_right[0] eps_right = qr[1:].reshape((3, 1)) elif qr.shape == (3, ): eta_right = 0 eps_right = qr.reshape((3, 1)) q_right = np.concatenate(([0], qr)) else: raise RuntimeError( f"utils.quaternion_product: Quaternion multiplication error, right quaternion wrong shape: {qr.shape}" ) quaternion = np.zeros((4, )) quaternion[0] = eta_left * eta_right - ql[1:] @ qr[1:] quaternion[1:4] = (eta_right * eps_left).reshape(3) + ( eta_left * eps_right ).reshape(3) + utils.cross_product_matrix(eps_left) @ q_right[1:4] # Ensure result is of correct shape quaternion = quaternion.ravel() assert quaternion.shape == ( 4, ), f"utils.quaternion_product: Quaternion multiplication error, result quaternion wrong shape: {quaternion.shape}" return quaternion
def quaternion_product(ql: np.ndarray, qr: np.ndarray) -> np.ndarray: """Perform quaternion product according to either (10.21) or (10.34). Args: ql (np.ndarray): Left quaternion of the product of either shape (3,) (pure quaternion) or (4,) qr (np.ndarray): Right quaternion of the product of either shape (3,) (pure quaternion) or (4,) Raises: RuntimeError: Left or right quaternion are of the wrong shape AssertionError: Resulting quaternion is of wrong shape Returns: np.ndarray: Quaternion product of ql and qr of shape (4,)s """ if ql.shape == (4, ): eta_left = ql[0] epsilon_left = ql[1:].reshape((3, 1)) elif ql.shape == (3, ): eta_left = 0 epsilon_left = ql.reshape((3, 1)) else: raise RuntimeError( f"utils.quaternion_product: Quaternion multiplication error, left quaternion shape incorrect: {ql.shape}" ) if qr.shape == (4, ): q_right = qr.copy() elif qr.shape == (3, ): q_right = np.concatenate(([0], qr)) else: raise RuntimeError( f"utils.quaternion_product: Quaternion multiplication error, right quaternion wrong shape: {qr.shape}" ) eps_matr = np.vstack( (np.hstack(([[0]], -1 * epsilon_left.T)), np.hstack((epsilon_left, cross_product_matrix(epsilon_left))))) # Eq. (10.34) : quaternion = (eta_left * np.eye(4) + eps_matr).dot(q_right) # Ensure result is of correct shape quaternion = quaternion.ravel() # print(quaternion) assert quaternion.shape == ( 4, ), f"utils.quaternion_product: Quaternion multiplication error, result quaternion wrong shape: {quaternion.shape}" return quaternion
def quaternion_product(ql: np.ndarray, qr: np.ndarray) -> np.ndarray: """Perform quaternion product according to either (10.21) or (10.34). Args: ql (np.ndarray): Left quaternion of the product of either shape (3,) (pure quaternion) or (4,) qr (np.ndarray): Right quaternion of the product of either shape (3,) (pure quaternion) or (4,) Raises: RuntimeError: Left or right quaternion are of the wrong shape AssertionError: Resulting quaternion is of wrong shape Returns: np.ndarray: Quaternion product of ql and qr of shape (4,)s """ if ql.shape == (4,): eta_left = ql[0] epsilon_left = ql[1:].reshape((3, 1)) elif ql.shape == (3,): eta_left = 0 epsilon_left = ql.reshape((3, 1)) else: raise RuntimeError( f"utils.quaternion_product: Quaternion multiplication error, left quaternion shape incorrect: {ql.shape}" ) if qr.shape == (4,): q_right = qr.copy() elif qr.shape == (3,): q_right = np.concatenate(([0], qr)) else: raise RuntimeError( f"utils.quaternion_product: Quaternion multiplication error, right quaternion wrong shape: {qr.shape}" ) # TODO: Muligens ikke riktig? mellomsteg = np.block([[0, -epsilon_left.T], [epsilon_left, utils.cross_product_matrix(epsilon_left)]]) quaternion = (eta_left * np.eye(4) + mellomsteg) @ q_right # Ensure result is of correct shape quaternion = quaternion.ravel() assert quaternion.shape == ( 4, ), f"utils.quaternion_product: Quaternion multiplication error, result quaternion wrong shape: {quaternion.shape}" return quaternion
def update_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Updates the state and covariance from a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to update, shape (16,) P (np.ndarray): The error state covariance to update, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). Defaults to np.zeros(3), shape (3,). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[np.ndarray, np.ndarray]: Updated Tuple(x_injected, P_injected): x_injected: The nominal state after injection of updated error state, shape (16,) P_injected: The error state covariance after error state update and injection, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.update_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.update_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.update_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.update_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.update_GNSS: lever_arm shape incorrect {lever_arm.shape}" I = np.eye(*P.shape) innovation, S = self.innovation_GNSS_position(x_nominal, P, z_GNSS_position, R_GNSS, lever_arm) # H_x = np.concatenate((np.identity(3), np.zeros((3,13))), axis=1) # Eq. (10.80) Measurement matrix # q = x_nominal[ATT_IDX] # eta = q[0] # epsilon = q[1:] # Q_bottom = eta * np.eye(3) + cross_product_matrix(epsilon) # Q_top = -epsilon.T # Q_deltaTheta = 0.5 * np.concatenate(([Q_top], Q_bottom), axis=0) # X_deltax = la.block_diag(np.identity(6), Q_deltaTheta, np.identity(6)) # H = H_x @ X_deltax H = np.concatenate((np.identity(3), np.zeros((3, 12))), axis=1) # in case of a specified lever arm if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) # KF error state update W = (la.solve( S.T, H @ P.T)).T # Kalman gain: W = P @ np.transpose(H) @ np.inv(S) #delta_x = x_nominal + W @ innovation # delta x ... think this is wrong !! delta_x = W @ innovation Jo = I - W @ H # for Joseph form P_update = Jo @ P @ np.transpose(Jo) + W @ R_GNSS @ np.transpose( W) # P update # error state injection x_injected, P_injected = self.inject(x_nominal, delta_x, P_update) assert x_injected.shape == ( 16, ), f"ESKF.update_GNSS: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.update_GNSS: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected
def innovation_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Calculates the innovation and its covariance for a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,) P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S): v: innovation, shape (3,) S: innovation covariance, shape (3, 3) """ assert x_nominal.shape == ( 16, ), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}" I = np.identity(3) # H_x = (np.concatenate((I, np.zeros((3,12))), axis = 1)) # Eq. (10.80) Measure position # q = x_nominal[ATT_IDX] # eta = q[0] # epsilon = q[1:] # Q_bottom = eta * np.eye(3) + cross_product_matrix(epsilon) # Q_top = -epsilon.T # Q_deltaTheta = 0.5 * np.concatenate(([Q_top], Q_bottom), axis=0) # X_deltax = la.block_diag(np.identity(6), Q_deltaTheta, np.identity(6)) # H = H_x @ X_deltax # z_pred = H_x @ x_nominal # Predicted measurement H = (np.concatenate((I, np.zeros((3, 12))), axis=1)) v = z_GNSS_position - x_nominal[POS_IDX] # z_pred # innovation # leverarm compensation if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) v -= R @ lever_arm S = H @ P @ H.T + R_GNSS # innovation covariance assert v.shape == ( 3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}" assert S.shape == ( 3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}" return v, S
def inject( self, x_nominal: np.ndarray, delta_x: np.ndarray, P: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray]: """Inject a calculated error state into the nominal state and compensate in the covariance. Args: x_nominal (np.ndarray): The nominal state to inject the error state deviation into, shape (16,) delta_x (np.ndarray): The error state deviation, shape (15,) P (np.ndarray): The error state covariance matrix Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Injected Tuple(x_injected, P_injected): x_injected: The injected nominal state, shape (16,) P_injected: The injected error state covariance matrix, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.inject: x_nominal shape incorrect {x_nominal.shape}" assert delta_x.shape == ( 15, ), f"ESKF.inject: delta_x shape incorrect {delta_x.shape}" assert P.shape == (15, 15), f"ESKF.inject: P shape incorrect {P.shape}" ### Useful concatenation of indices # All injection indices, minus the attitude INJ_IDX = POS_IDX + VEL_IDX + ACC_BIAS_IDX + GYRO_BIAS_IDX # All error indices, minus the attitude DTX_IDX = POS_IDX + VEL_IDX + ERR_ACC_BIAS_IDX + ERR_GYRO_BIAS_IDX x_injected = x_nominal.copy() x_injected[INJ_IDX] += delta_x[ DTX_IDX] # TODO: Inject error state into nominal state (except attitude / quaternion) quat = quaternion_product( x_nominal[ATT_IDX], np.concatenate( ([1], (0.5 * delta_x[ERR_ATT_IDX])))) # TODO: Inject attitude quat = quat / la.norm(quat) # TODO: Normalize quaternion x_injected[ATT_IDX] = quat # Covariance G_injected = np.zeros( (15, 15)) # TODO: Compensate for injection in the covariances G_injected[:6, :6] = np.eye(6) G_injected[9:, 9:] = np.eye(6) G_injected[6:9, 6:9] = np.eye(3) - cross_product_matrix( 0.5 * delta_x[ERR_ATT_IDX]) P_injected = G_injected @ P @ G_injected.T # TODO: Compensate for injection in the covariances assert x_injected.shape == ( 16, ), f"ESKF.inject: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.inject: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected
def update_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Updates the state and covariance from a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to update, shape (16,) P (np.ndarray): The error state covariance to update, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). Defaults to np.zeros(3), shape (3,). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[np.ndarray, np.ndarray]: Updated Tuple(x_injected, P_injected): x_injected: The nominal state after injection of updated error state, shape (16,) P_injected: The error state covariance after error state update and injection, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.update_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.update_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.update_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.update_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.update_GNSS: lever_arm shape incorrect {lever_arm.shape}" I = np.eye(*P.shape) innovation, S = self.innovation_GNSS_position(x_nominal, P, z_GNSS_position, R_GNSS, lever_arm) Hx = np.zeros((3, 16)) # TODO: measurement matrix Hx[:, 0:3] = np.eye(3) q = x_nominal[ATT_IDX] X = np.zeros((16, 15)) X[0:6, 0:6] = np.eye(6) X[10:16, 9:15] = np.eye(6) X[6, 6:9] = 0.5 * np.array([-q[1], -q[2], -q[3]]) X[7, 6:9] = 0.5 * np.array([q[0], -q[3], q[2]]) X[8, 6:9] = 0.5 * np.array([q[3], q[0], -q[1]]) X[9, 6:9] = 0.5 * np.array([-q[2], q[1], q[0]]) H = Hx @ X #sverre: 10.76 # H = np.block([np.eye(3), np.zeros((3,12))]) # Simon: Hvorfor denne H-en?? # in case of a specified lever arm if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) # KF error state update W = P @ H.T @ np.linalg.inv(S) # sverre: 10.75 TODO: Kalman gain #delta_x = np.zeros((15,)) # TODO: delta x delta_x = W @ innovation #sverre: delta_x (15,) Jo = I - W @ H # for Joseph form # TODO: P update # P_update = Jo @ P @ Jo.T + W @ R @ W.T #sverre: 4.10 (numerically faster) (jospeh form) P_update = Jo @ P @ Jo.T + W @ R_GNSS @ W.T #sverre: 4.10 (numerically faster) (jospeh form) # Simon: Hvorfor R_GNSS?? # error state injection x_injected, P_injected = self.inject(x_nominal, delta_x, P_update) assert x_injected.shape == ( 16, ), f"ESKF.update_GNSS: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.update_GNSS: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected
def update_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Updates the state and covariance from a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to update, shape (16,) P (np.ndarray): The error state covariance to update, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). Defaults to np.zeros(3), shape (3,). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[np.ndarray, np.ndarray]: Updated Tuple(x_injected, P_injected): x_injected: The nominal state after injection of updated error state, shape (16,) P_injected: The error state covariance after error state update and injection, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.update_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.update_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.update_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.update_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.update_GNSS: lever_arm shape incorrect {lever_arm.shape}" I = np.eye(*P.shape) innovation, S = self.innovation_GNSS_position(x_nominal, P, z_GNSS_position, R_GNSS, lever_arm) # TODO: measurement matrix H = np.zeros((3, 15)) H[POS_IDX * POS_IDX] = np.eye(3) # in case of a specified lever arm if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) # KF error state update # TODO: Kalman gain W = P @ H.T @ la.inv(S) # TODO: delta x delta_x = W @ innovation Jo = I - W @ H # for Joseph form # TODO: P update P_update = Jo @ P @ Jo.T + W @ R_GNSS @ W.T # error state injection x_injected, P_injected = self.inject(x_nominal, delta_x, P_update) assert x_injected.shape == ( 16, ), f"ESKF.update_GNSS: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.update_GNSS: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected
def innovation_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Calculates the innovation and its covariance for a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,) P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S): v: innovation, shape (3,) S: innovation covariance, shape (3, 3) """ assert x_nominal.shape == ( 16, ), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}" #: measurement matrix, only measures position Hx = np.concatenate((np.eye(3), np.zeros((3, 13))), axis=1) #: innovation v = z_GNSS_position - x_nominal[POS_IDX] #: innovation covariance eta, e1, e2, e3 = x_nominal[ATT_IDX].ravel() Q_dt = 0.5 * np.array([ -e1, -e2, -e3, eta, -e3, e2, e3, eta, -e1, -e2, e1, eta ]).reshape(4, 3) X_dx = la.block_diag(np.eye(6), Q_dt, np.eye(6)) H = Hx @ X_dx # leverarm compensation if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) v -= R @ lever_arm S = H @ P @ H.T + R_GNSS assert v.shape == ( 3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}" assert S.shape == ( 3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}" return v, S, H
def innovation_GNSS_position( self, x_nominal: np.ndarray, P: np.ndarray, z_GNSS_position: np.ndarray, R_GNSS: np.ndarray, lever_arm: np.ndarray = np.zeros(3), ) -> Tuple[np.ndarray, np.ndarray]: """Calculates the innovation and its covariance for a GNSS position measurement Args: x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,) P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15) z_GNSS_position (np.ndarray): The measured 3D position, shape (3,) R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3) lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3). Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S): v: innovation, shape (3,) S: innovation covariance, shape (3, 3) """ assert x_nominal.shape == ( 16, ), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}" assert P.shape == ( 15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}" assert z_GNSS_position.shape == ( 3, ), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}" assert R_GNSS.shape == ( 3, 3, ), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}" assert lever_arm.shape == ( 3, ), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}" Hx = np.zeros((3, 16)) # TODO: measurement matrix Hx[:, 0:3] = np.eye(3) q = x_nominal[ATT_IDX] X = np.zeros((16, 15)) X[0:6, 0:6] = np.eye(6) X[10:16, 9:15] = np.eye(6) X[6, 6:9] = 0.5 * np.array([-q[1], -q[2], -q[3]]) X[7, 6:9] = 0.5 * np.array([q[0], -q[3], q[2]]) X[8, 6:9] = 0.5 * np.array([q[3], q[0], -q[1]]) X[9, 6:9] = 0.5 * np.array([-q[2], q[1], q[0]]) H = Hx @ X #sverre: 10.76 v = z_GNSS_position - Hx @ x_nominal # sverre: bruker Hx her siden den er (3,16) TODO: innovation # H_2 = np.block([np.eye(3), np.zeros((3,12))]) # Simon: Hvorfor denne H-en? # v_2 = z_GNSS_position - x_nominal[POS_IDX] # Simon: Hvorfor denne v-en? # leverarm compensation if not np.allclose(lever_arm, 0): R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm, debug=self.debug) v -= R @ lever_arm S = H @ P @ H.T + R_GNSS # TODO: innovation covariance assert v.shape == ( 3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}" assert S.shape == ( 3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}" return v, S
def inject( self, x_nominal: np.ndarray, delta_x: np.ndarray, P: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray]: """Inject a calculated error state into the nominal state and compensate in the covariance. Args: x_nominal (np.ndarray): The nominal state to inject the error state deviation into, shape (16,) delta_x (np.ndarray): The error state deviation, shape (15,) P (np.ndarray): The error state covariance matrix Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: Tuple[ np.ndarray, np.ndarray ]: Injected Tuple(x_injected, P_injected): x_injected: The injected nominal state, shape (16,) P_injected: The injected error state covariance matrix, shape (15, 15) """ assert x_nominal.shape == ( 16, ), f"ESKF.inject: x_nominal shape incorrect {x_nominal.shape}" assert delta_x.shape == ( 15, ), f"ESKF.inject: delta_x shape incorrect {delta_x.shape}" assert P.shape == (15, 15), f"ESKF.inject: P shape incorrect {P.shape}" # All injection indices, minus the attitude INJ_IDX = POS_IDX + VEL_IDX + ACC_BIAS_IDX + GYRO_BIAS_IDX # All error indices, minus the attitude DTX_IDX = POS_IDX + VEL_IDX + ERR_ACC_BIAS_IDX + ERR_GYRO_BIAS_IDX x_injected = x_nominal.copy() # Inject error state into nominal state (except attitude / quaternion) x_injected[INJ_IDX] += delta_x[DTX_IDX] # Inject attitude dx_quat = euler_to_quaternion(delta_x[ERR_ATT_IDX]) x_injected[ATT_IDX] = quaternion_product( x_nominal[ATT_IDX], dx_quat) # Normalize quaternion x_injected[ATT_IDX] = x_injected[ATT_IDX] / \ la.norm(x_injected[ATT_IDX]) # Covariance # Compensate for injection in the covariances # Implements Eq. (10.86) : G_injected = la.block_diag(np.eye(6), np.eye( 3) - cross_product_matrix(1/2*delta_x[ERR_ATT_IDX]), np.eye(6)) P_injected = G_injected @ P @ G_injected.T assert x_injected.shape == ( 16, ), f"ESKF.inject: x_injected shape incorrect {x_injected.shape}" assert P_injected.shape == ( 15, 15, ), f"ESKF.inject: P_injected shape incorrect {P_injected.shape}" return x_injected, P_injected