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 delta_x( cls, x_nominal: np.ndarray, x_true: np.ndarray, ) -> np.ndarray: """Calculates the error state between x_nominal and x_true Args: x_nominal (np.ndarray): The nominal estimated state, shape (16,) x_true (np.ndarray): The true 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 state difference in error state, shape (15,) """ assert x_nominal.shape == ( 16, ), f"ESKF.delta_x: x_nominal shape incorrect {x_nominal.shape}" assert x_true.shape == ( 16, ), f"ESKF.delta_x: x_true shape incorrect {x_true.shape}" delta_position = x_true[POS_IDX] - x_nominal[ POS_IDX] # TODO: Delta position delta_velocity = x_true[VEL_IDX] - x_nominal[ VEL_IDX] # TODO: Delta velocity quaternion_conj = x_nominal[ATT_IDX] quaternion_conj[1:4] = -1 * quaternion_conj[ 1:4] # TODO: Conjugate of quaternion delta_quaternion = quaternion_product( quaternion_conj, x_true[ATT_IDX]) # TODO: Error quaternion # if delta_quaternion[0] < 0: # delta_quaternion = -delta_quaternion delta_theta = 2 * delta_quaternion[ 1: 4] #sverre: brukte hinte i oppgaven Im(delta_q) ~ 0.5*delta_theta # Concatenation of bias indices BIAS_IDX = ACC_BIAS_IDX + GYRO_BIAS_IDX delta_bias = x_true[BIAS_IDX] - x_nominal[ BIAS_IDX] # TODO: Error biases d_x = np.concatenate( (delta_position, delta_velocity, delta_theta, delta_bias)) assert d_x.shape == ( 15, ), f"ESKF.delta_x: d_x shape incorrect {d_x.shape}" return d_x
def delta_x( cls, x_nominal: np.ndarray, x_true: np.ndarray, ) -> np.ndarray: """Calculates the error state between x_nominal and x_true Args: x_nominal (np.ndarray): The nominal estimated state, shape (16,) x_true (np.ndarray): The true 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 state difference in error state, shape (15,) """ assert x_nominal.shape == ( 16, ), f"ESKF.delta_x: x_nominal shape incorrect {x_nominal.shape}" assert x_true.shape == ( 16, ), f"ESKF.delta_x: x_true shape incorrect {x_true.shape}" delta_position = x_true[POS_IDX] - x_nominal[POS_IDX] delta_velocity = x_true[VEL_IDX] - x_nominal[VEL_IDX] quaternion_conj = -x_nominal[ATT_IDX] quaternion_conj[0] = -quaternion_conj[0] # TODO: Error quaternion delta_quaternion = quaternion_product(quaternion_conj, x_true[ATT_IDX]) quat_norm = la.norm(delta_quaternion) delta_quaternion = delta_quaternion / quat_norm delta_theta = 2 * delta_quaternion[ 1:] # Perhaps quat2euler or something? # Concatenation of bias indices BIAS_IDX = ACC_BIAS_IDX + GYRO_BIAS_IDX # TODO: Error biases delta_bias = x_true[BIAS_IDX] - x_nominal[BIAS_IDX] d_x = np.concatenate( (delta_position, delta_velocity, delta_theta, delta_bias)) assert d_x.shape == ( 15, ), f"ESKF.delta_x: d_x shape incorrect {d_x.shape}" return d_x
def delta_x( cls, x_nominal: np.ndarray, x_true: np.ndarray, ) -> np.ndarray: """Calculates the error state between x_nominal and x_true Args: x_nominal (np.ndarray): The nominal estimated state, shape (16,) x_true (np.ndarray): The true 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 state difference in error state, shape (15,) """ assert x_nominal.shape == ( 16, ), f"ESKF.delta_x: x_nominal shape incorrect {x_nominal.shape}" assert x_true.shape == ( 16, ), f"ESKF.delta_x: x_true shape incorrect {x_true.shape}" delta_position = x_true[POS_IDX] - x_nominal[ POS_IDX] # Eller motsatt? TODO: Delta position delta_velocity = x_true[VEL_IDX] - x_nominal[ VEL_IDX] # TODO: Delta velocity #quaternion_conj = np.vstack((x_nominal[6], -x_nominal[7:10])) # TODO: Conjugate of quaternion quaternion_conj = np.array( [x_nominal[6], -x_nominal[7], -x_nominal[8], -x_nominal[9]]) delta_quaternion = quaternion_product( quaternion_conj, x_true[ATT_IDX] ) # why do I need Im(delta_quaternion) = 0.5*delta_theta # TODO: Error quaternion delta_theta = 2 * delta_quaternion[1:] # Hmmmm... # Concatenation of bias indices BIAS_IDX = ACC_BIAS_IDX + GYRO_BIAS_IDX delta_bias = x_true[BIAS_IDX] - x_nominal[ BIAS_IDX] # TODO: Error biases d_x = np.concatenate( (delta_position, delta_velocity, delta_theta, delta_bias)) assert d_x.shape == ( 15, ), f"ESKF.delta_x: d_x shape incorrect {d_x.shape}" return d_x
def delta_x(cls, x_nominal: np.ndarray, x_true: np.ndarray,) -> np.ndarray: """Calculates the error state between x_nominal and x_true Args: x_nominal (np.ndarray): The nominal estimated state, shape (16,) x_true (np.ndarray): The true 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 state difference in error state, shape (15,) """ assert x_nominal.shape == ( 16, ), f"ESKF.delta_x: x_nominal shape incorrect {x_nominal.shape}" assert x_true.shape == ( 16, ), f"ESKF.delta_x: x_true shape incorrect {x_true.shape}" delta_position = x_true[POS_IDX] - x_nominal[POS_IDX] delta_velocity = x_true[VEL_IDX] - x_nominal[VEL_IDX] # Conjugate of nominal quaternion quaternion_conj = np.array((1, *[-1]*3)) * x_nominal[ATT_IDX] # Error quaternion delta_quaternion = quaternion_product(quaternion_conj, x_true[ATT_IDX]) # The error state delta_theta = quaternion_to_euler(delta_quaternion) # Concatenation of bias indices BIAS_IDX = ACC_BIAS_IDX + GYRO_BIAS_IDX delta_bias = x_true[BIAS_IDX] - x_nominal[BIAS_IDX] d_x = np.concatenate( (delta_position, delta_velocity, delta_theta, delta_bias)) assert d_x.shape == ( 15,), f"ESKF.delta_x: d_x shape incorrect {d_x.shape}" return d_x
def delta_x(cls, x_nominal: np.ndarray, x_true: np.ndarray,) -> np.ndarray: """Calculates the error state between x_nominal and x_true Args: x_nominal (np.ndarray): The nominal estimated state, shape (16,) x_true (np.ndarray): The true 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 state difference in error state, shape (15,) """ assert x_nominal.shape == ( 16, ), f"ESKF.delta_x: x_nominal shape incorrect {x_nominal.shape}" assert x_true.shape == ( 16, ), f"ESKF.delta_x: x_true shape incorrect {x_true.shape}" # Lecture 8C, slide 5 # δx = [δρ ; δv ; δθ ; δab ; δωb] delta_position = x_true[POS_IDX] - x_nominal[POS_IDX] delta_velocity = x_true[VEL_IDX] - x_nominal[VEL_IDX] quaternion_conj = np.diag([1,-1,-1,-1]) @ x_nominal[ATT_IDX] quaternion_inv = quaternion_conj/la.norm(x_nominal[ATT_IDX]) delta_quaternion = quaternion_product(quaternion_inv, x_true[ATT_IDX]) delta_theta = delta_quaternion[1:]*2 # Concatenation of bias indices BIAS_IDX = ACC_BIAS_IDX + GYRO_BIAS_IDX delta_bias = x_true[BIAS_IDX]-x_nominal[BIAS_IDX] # TODO: Error biases d_x = np.concatenate((delta_position, delta_velocity, delta_theta, delta_bias)) assert d_x.shape == (15,), f"ESKF.delta_x: d_x shape incorrect {d_x.shape}" return d_x
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 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 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 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 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
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) 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 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 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