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
