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