def Gerr( self, x_nominal: np.ndarray, ) -> np.ndarray: """Calculate the continuous time error state noise input matrix Args: x_nominal (np.ndarray): The nominal state, shape (16,) Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The continuous time error state noise input matrix, shape (15, 12) """ assert x_nominal.shape == ( 16, ), f"ESKF.Gerr: x_nominal shape incorrect {x_nominal.shape}" R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) G = np.zeros((15, 12)) G[VEL_IDX * POS_IDX] = -R G[ERR_ATT_IDX * VEL_IDX] = -np.eye(3) G[ERR_ACC_BIAS_IDX * ERR_ATT_IDX] = np.eye(3) G[ERR_GYRO_BIAS_IDX * ERR_ACC_BIAS_IDX] = np.eye(3) assert G.shape == ( 15, 12), f"ESKF.Gerr: G-matrix shape incorrect {G.shape}" return G
def Gerr( self, x_nominal: np.ndarray, ) -> np.ndarray: """Calculate the continuous time error state noise input matrix Args: x_nominal (np.ndarray): The nominal state, shape (16,) Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The continuous time error state noise input matrix, shape (15, 12) """ assert x_nominal.shape == ( 16, ), f"ESKF.Gerr: x_nominal shape incorrect {x_nominal.shape}" R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug) I = np.identity(3) # G in eq. (10.68) G = np.vstack((np.zeros((3, 12)), la.block_diag(-R, -I, I, I))) assert G.shape == ( 15, 12), f"ESKF.Gerr: G-matrix shape incorrect {G.shape}" return G
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 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 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] = -1 * R @ cross_product_matrix(acceleration) A[VEL_IDX * ERR_ACC_BIAS_IDX] = -1 * R A[ERR_ATT_IDX * ERR_ATT_IDX] = -1 * cross_product_matrix(omega) A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = - 1 * np.eye(3) A[ERR_ACC_BIAS_IDX * ERR_ACC_BIAS_IDX] = -1 * self.p_acc * np.eye(3) A[ERR_GYRO_BIAS_IDX * ERR_GYRO_BIAS_IDX] = -1 * 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 predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion**2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) a = R @ acceleration + self.g # Already debiased, apparently k = Ts * omega kn = la.norm(k) position_prediction = position + Ts * velocity + ( Ts**2 / 2) * a # np.zeros((3,)) # TODO: Calculate predicted position velocity_prediction = velocity + Ts * a # np.zeros((3,)) # TODO: Calculate predicted velocity quaternion_prediction = quaternion_product( quaternion, np.concatenate(([np.cos(kn / 2)], np.sin(kn / 2) * (k.T / kn))) ) # TODO: Calculate predicted quaternion, might need to normalize omegas # Normalize quaternion quaternion_prediction = quaternion_prediction / ( la.norm(quaternion_prediction)) # TODO: Normalize acceleration_bias_prediction = acceleration_bias # assuming p_acc = 0 TODO: Calculate predicted acceleration bias gyroscope_bias_prediction = gyroscope_bias # assuming p_gyro = 0 TODO: Calculate predicted gyroscope bias x_nominal_predicted = np.concatenate(( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, )) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted
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 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 predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion**2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) position_prediction = position + Ts * velocity + Ts**2 * 0.5 * ( R @ acceleration + self.g ) # sverre: velocity er allerede i WORLD TODO: Calculate predicted position # TODO: Calculate predicted velocity velocity_prediction = velocity + Ts * ( R @ acceleration + self.g ) #skal denne roteres når det antas at a = R(q)(a_m - a_b) + g? # TODO: Calculate predicted quaternion kappa = Ts * omega # sverre: står i hintet at denne skal oppgis i BODY kappa_norm = np.linalg.norm( kappa ) # sverre: dette skal være en 2-norm #############er dette riktig norm?########## qr = np.zeros(4) # qr[0] = np.cos(0.5*kappa_norm) qr[0] = np.cos(0.5 * kappa_norm) qr[1:4] = np.sin(0.5 * kappa_norm) * (kappa.T / kappa_norm) quaternion_prediction = quaternion_product( quaternion, qr.T ) # sverre: skal denne uttrykkes i body eller world? Er forskjellige definisjoner av q_dot i boka og i hintet til oppgaven. # Normalize quaternion quaternion_prediction = quaternion_prediction / np.linalg.norm( quaternion_prediction) # TODO: Normalize acceleration_bias_prediction = acceleration_bias - Ts * self.p_acc * acceleration_bias # TODO: Calculate predicted acceleration bias gyroscope_bias_prediction = gyroscope_bias - Ts * self.p_gyro * gyroscope_bias # TODO: Calculate predicted gyroscope bias x_nominal_predicted = np.concatenate(( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, )) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted
def predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the ion interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion**2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) position_prediction = position + Ts * velocity + ( (1 / 2) * Ts**2) * (R @ acceleration + self.g) velocity_prediction = velocity + Ts * (R @ acceleration + self.g) k = Ts * omega k_norm_2 = la.norm(k, 2) rhs_quat = np.array( [np.cos(k_norm_2 / 2), *np.sin(k_norm_2 / 2) * (k.T) / k_norm_2]) rhs_quat = rhs_quat.T # Normalize quaternion quaternion_prediction = quaternion_product(quaternion, rhs_quat) # TODO: Normalize quaternion_prediction /= la.norm(quaternion_prediction, 2) p_ab = self.p_acc p_wb = self.p_gyro acceleration_bias_prediction = acceleration_bias * np.exp( -Ts * p_ab) #acceleration_bias*exp(-Ts*p_ab) gyroscope_bias_prediction = gyroscope_bias * np.exp(-Ts * p_wb) x_nominal_predicted = np.concatenate(( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, )) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted
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 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 predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion ** 2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) position_prediction = position + Ts*velocity + (Ts**2*(R@acceleration + self.g))/2 velocity_prediction = velocity +Ts*(R@acceleration + self.g) vec_inc = Ts*omega vec_inc_norm = la.norm(vec_inc) r_quat = np.array([np.cos(vec_inc_norm/2), *(vec_inc.T * np.sin(vec_inc_norm/2))/vec_inc_norm]).T quaternion_prediction = quaternion_product(quaternion, r_quat) # dobbeltsjekk # Normalize quaternion quaternion_prediction = quaternion_prediction/la.norm(quaternion_prediction) # acceleration_bias_prediction = acceleration_bias - Ts * self.p_acc * acceleration_bias # gyroscope_bias_prediction = gyroscope_bias - Ts * self.p_gyro * gyroscope_bias #? Spør studass her acceleration_bias_prediction = acceleration_bias*np.exp(-self.p_acc*Ts) # potensielt += her gyroscope_bias_prediction = gyroscope_bias*np.exp(-self.p_gyro*Ts) # potensielt += her x_nominal_predicted = np.concatenate( ( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, ) ) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted
def predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion**2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) g = np.array([0, 0, 9.81]) acceleration_world = R @ acceleration + g # omega_world = R @ omega position_prediction = position + Ts * velocity + Ts**2 / 2 * acceleration_world velocity_prediction = velocity + Ts * acceleration_world kappa = Ts * omega kappa_norm = la.norm(kappa) delta_quat = np.array([ np.cos(kappa_norm / 2), *(np.sin(kappa_norm / 2) * kappa.T / kappa_norm) ]) quaternion_prediction = quaternion_product(quaternion, delta_quat) # Normalize quaternion quaternion_prediction = quaternion_prediction / la.norm( quaternion_prediction) acceleration_bias_prediction = (1 - Ts * self.p_acc) * acceleration_bias gyroscope_bias_prediction = (1 - Ts * self.p_gyro) * gyroscope_bias x_nominal_predicted = np.concatenate(( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, )) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted
def predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion**2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) g = np.array([0, 0, 9.81]) position_prediction = position + velocity * Ts + 1 / 2 * ( R @ acceleration + g) * Ts**2 velocity_prediction = velocity + (R @ acceleration + g) * Ts k = omega * Ts k_norm = la.norm(k) e = np.array( [np.cos(k_norm / 2), *(np.sin(k_norm / 2) * k.T / k_norm)]).T quaternion_prediction = quaternion_product(quaternion, e) quaternion_prediction = quaternion_prediction / la.norm( quaternion_prediction) #I = np.eye(3) #acceleration_bias_prediction = acceleration_bias - self.p_acc * I @ acceleration_bias * Ts #gyroscope_bias_prediction = gyroscope_bias - self.p_gyro * I @ gyroscope_bias * Ts acceleration_bias_prediction = acceleration_bias * np.exp( -self.p_acc * Ts) gyroscope_bias_prediction = gyroscope_bias * np.exp(-self.p_gyro * Ts) x_nominal_predicted = np.concatenate(( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, )) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted
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 predict_nominal( self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray, Ts: float, ) -> np.ndarray: """Discrete time prediction, equation (10.58) Args: x_nominal (np.ndarray): The nominal state to predict, shape (16,) acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,) omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,) Ts (float): The sampling time Raises: AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties Returns: np.ndarray: The predicted nominal state, shape (16,) """ assert x_nominal.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}" assert acceleration.shape == ( 3, ), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}" assert omega.shape == ( 3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}" # Extract states position = x_nominal[POS_IDX] velocity = x_nominal[VEL_IDX] quaternion = x_nominal[ATT_IDX] acceleration_bias = x_nominal[ACC_BIAS_IDX] gyroscope_bias = x_nominal[GYRO_BIAS_IDX] if self.debug: assert np.allclose( np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized." assert np.allclose( np.sum(quaternion**2), 1, rtol=0, atol=1e-15 ), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it." R = quaternion_to_rotation_matrix(quaternion, debug=self.debug) # Assumptions a = R @ acceleration + self.g w = omega # Update predictions for new time steps using the hint position_prediction = position + Ts * velocity + Ts**2 * a / 2 velocity_prediction = velocity + Ts * a # Update quaternion using the hint kappa = Ts * w kappa_norm = np.linalg.norm(kappa) exponent = np.array([ np.cos(kappa_norm / 2), *(np.sin(kappa_norm / 2) * (kappa.T) / kappa_norm) ]).T quaternion_prediction = quaternion_product(quaternion, exponent) # Normalize quaternion quaternion_prediction = quaternion_prediction / np.linalg.norm( quaternion_prediction) # Update biases acceleration_bias_prediction = acceleration_bias * np.exp( -self.p_acc * Ts) gyroscope_bias_prediction = gyroscope_bias * np.exp(-self.p_gyro * Ts) x_nominal_predicted = np.concatenate(( position_prediction, velocity_prediction, quaternion_prediction, acceleration_bias_prediction, gyroscope_bias_prediction, )) assert x_nominal_predicted.shape == ( 16, ), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}" return x_nominal_predicted