def Gerr(
self,
x_nominal: np.ndarray,
) -> np.ndarray:
"""Calculate the continuous time error state noise input matrix
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The continuous time error state noise input matrix, shape (15, 12)
"""
assert x_nominal.shape == (
16, ), f"ESKF.Gerr: x_nominal shape incorrect {x_nominal.shape}"
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug)
G = np.zeros((15, 12))
G[VEL_IDX * POS_IDX] = -R
G[ERR_ATT_IDX * VEL_IDX] = -np.eye(3)
G[ERR_ACC_BIAS_IDX * ERR_ATT_IDX] = np.eye(3)
G[ERR_GYRO_BIAS_IDX * ERR_ACC_BIAS_IDX] = np.eye(3)
assert G.shape == (
15, 12), f"ESKF.Gerr: G-matrix shape incorrect {G.shape}"
return G
def Gerr(
self,
x_nominal: np.ndarray,
) -> np.ndarray:
"""Calculate the continuous time error state noise input matrix
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The continuous time error state noise input matrix, shape (15, 12)
"""
assert x_nominal.shape == (
16, ), f"ESKF.Gerr: x_nominal shape incorrect {x_nominal.shape}"
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug)
I = np.identity(3)
# G in eq. (10.68)
G = np.vstack((np.zeros((3, 12)), la.block_diag(-R, -I, I, I)))
assert G.shape == (
15, 12), f"ESKF.Gerr: G-matrix shape incorrect {G.shape}"
return G
def Aerr( # sverre: error state transition matrix
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
) -> np.ndarray:
"""Calculate the continuous time error state dynamics Jacobian.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration for the prediction interval, shape (3,)
omega (np.ndarray): The estimated rotation rate for the prediction interval, shape (3,)
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: Continuous time error state dynamics Jacobian, shape (15, 15)
"""
assert x_nominal.shape == (
16, ), f"ESKF.Aerr: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.Aerr: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.Aerr: omega shape incorrect {omega.shape}"
# Rotation matrix
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug)
# Allocate the matrix
A = np.zeros((15, 15))
# Set submatrices
A[POS_IDX * VEL_IDX] = np.eye(3)
A[VEL_IDX * ERR_ATT_IDX] = -R @ cross_product_matrix(acceleration)
A[VEL_IDX * ERR_ACC_BIAS_IDX] = -R
A[ERR_ATT_IDX * ERR_ATT_IDX] = -cross_product_matrix(
omega
) #tror vi holder oss til omega i BODY her (på samme måte som for quaternion_prediction over)
A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = -np.eye(3)
A[ERR_ACC_BIAS_IDX * ERR_ACC_BIAS_IDX] = -self.p_acc * np.eye(
3
) #p_acc er en proporsjonalitetskonstant (tuning-parameter) som bestermmer raten til biaset
A[ERR_GYRO_BIAS_IDX *
ERR_GYRO_BIAS_IDX] = -self.p_gyro * np.eye(3) #det samme gjelder her
# Bias correction
A[VEL_IDX *
ERR_ACC_BIAS_IDX] = A[VEL_IDX * ERR_ACC_BIAS_IDX] @ self.S_a
A[ERR_ATT_IDX *
ERR_GYRO_BIAS_IDX] = (A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] @ self.S_g)
assert A.shape == (
15,
15,
), f"ESKF.Aerr: A-error matrix shape incorrect {A.shape}"
return A
def innovation_GNSS_position(
self,
x_nominal: np.ndarray,
P: np.ndarray,
z_GNSS_position: np.ndarray,
R_GNSS: np.ndarray,
lever_arm: np.ndarray = np.zeros(3),
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculates the innovation and its covariance for a GNSS position measurement
Args:
x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,)
P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15)
z_GNSS_position (np.ndarray): The measured 3D position, shape (3,)
R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3).
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S):
v: innovation, shape (3,)
S: innovation covariance, shape (3, 3)
"""
assert x_nominal.shape == (
16,
), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (
15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}"
assert z_GNSS_position.shape == (
3,
), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}"
assert R_GNSS.shape == (
3,
3,
), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}"
assert lever_arm.shape == (
3,
), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}"
H = np.eye(3, 15)
v = z_GNSS_position - x_nominal[POS_IDX] # TODO: innovation
# leverarm compensation
if not np.allclose(lever_arm, 0):
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX],
debug=self.debug)
H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm,
debug=self.debug)
v -= R @ lever_arm
S = H @ P @ H.T + R_GNSS # TODO: innovation covariance
assert v.shape == (
3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}"
assert S.shape == (
3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}"
return v, S
def Aerr(
self, x_nominal: np.ndarray, acceleration: np.ndarray, omega: np.ndarray,
) -> np.ndarray:
"""Calculate the continuous time error state dynamics Jacobian.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration for the prediction interval, shape (3,)
omega (np.ndarray): The estimated rotation rate for the prediction interval, shape (3,)
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: Continuous time error state dynamics Jacobian, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.Aerr: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.Aerr: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3,), f"ESKF.Aerr: omega shape incorrect {omega.shape}"
# Rotation matrix
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX], debug=self.debug)
# Allocate the matrix
A = np.zeros((15, 15))
# Set submatrices
A[POS_IDX * VEL_IDX] = np.eye(3)
A[VEL_IDX * ERR_ATT_IDX] = -1 * R @ cross_product_matrix(acceleration)
A[VEL_IDX * ERR_ACC_BIAS_IDX] = -1 * R
A[ERR_ATT_IDX * ERR_ATT_IDX] = -1 * cross_product_matrix(omega)
A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = - 1 * np.eye(3)
A[ERR_ACC_BIAS_IDX * ERR_ACC_BIAS_IDX] = -1 * self.p_acc * np.eye(3)
A[ERR_GYRO_BIAS_IDX * ERR_GYRO_BIAS_IDX] = -1 * self.p_gyro * np.eye(3)
# Bias correction
A[VEL_IDX * ERR_ACC_BIAS_IDX] = A[VEL_IDX * ERR_ACC_BIAS_IDX] @ self.S_a
A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] = (
A[ERR_ATT_IDX * ERR_GYRO_BIAS_IDX] @ self.S_g
)
assert A.shape == (
15,
15,
), f"ESKF.Aerr: A-error matrix shape incorrect {A.shape}"
return A
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0,
atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion**2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
a = R @ acceleration + self.g # Already debiased, apparently
k = Ts * omega
kn = la.norm(k)
position_prediction = position + Ts * velocity + (
Ts**2 /
2) * a # np.zeros((3,)) # TODO: Calculate predicted position
velocity_prediction = velocity + Ts * a # np.zeros((3,)) # TODO: Calculate predicted velocity
quaternion_prediction = quaternion_product(
quaternion,
np.concatenate(([np.cos(kn / 2)], np.sin(kn / 2) * (k.T / kn)))
) # TODO: Calculate predicted quaternion, might need to normalize omegas
# Normalize quaternion
quaternion_prediction = quaternion_prediction / (
la.norm(quaternion_prediction)) # TODO: Normalize
acceleration_bias_prediction = acceleration_bias # assuming p_acc = 0 TODO: Calculate predicted acceleration bias
gyroscope_bias_prediction = gyroscope_bias # assuming p_gyro = 0 TODO: Calculate predicted gyroscope bias
x_nominal_predicted = np.concatenate((
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
))
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
def innovation_GNSS_position(
self,
x_nominal: np.ndarray,
P: np.ndarray,
z_GNSS_position: np.ndarray,
R_GNSS: np.ndarray,
lever_arm: np.ndarray = np.zeros(3),
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculates the innovation and its covariance for a GNSS position measurement
Args:
x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,)
P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15)
z_GNSS_position (np.ndarray): The measured 3D position, shape (3,)
R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3).
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S):
v: innovation, shape (3,)
S: innovation covariance, shape (3, 3)
"""
assert x_nominal.shape == (
16,
), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (
15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}"
assert z_GNSS_position.shape == (
3,
), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}"
assert R_GNSS.shape == (
3,
3,
), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}"
assert lever_arm.shape == (
3,
), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}"
I = np.identity(3)
# H_x = (np.concatenate((I, np.zeros((3,12))), axis = 1)) # Eq. (10.80) Measure position
# q = x_nominal[ATT_IDX]
# eta = q[0]
# epsilon = q[1:]
# Q_bottom = eta * np.eye(3) + cross_product_matrix(epsilon)
# Q_top = -epsilon.T
# Q_deltaTheta = 0.5 * np.concatenate(([Q_top], Q_bottom), axis=0)
# X_deltax = la.block_diag(np.identity(6), Q_deltaTheta, np.identity(6))
# H = H_x @ X_deltax
# z_pred = H_x @ x_nominal # Predicted measurement
H = (np.concatenate((I, np.zeros((3, 12))), axis=1))
v = z_GNSS_position - x_nominal[POS_IDX] # z_pred # innovation
# leverarm compensation
if not np.allclose(lever_arm, 0):
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX],
debug=self.debug)
H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm,
debug=self.debug)
v -= R @ lever_arm
S = H @ P @ H.T + R_GNSS # innovation covariance
assert v.shape == (
3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}"
assert S.shape == (
3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}"
return v, S
def update_GNSS_position(
self,
x_nominal: np.ndarray,
P: np.ndarray,
z_GNSS_position: np.ndarray,
R_GNSS: np.ndarray,
lever_arm: np.ndarray = np.zeros(3),
) -> Tuple[np.ndarray, np.ndarray]:
"""Updates the state and covariance from a GNSS position measurement
Args:
x_nominal (np.ndarray): The nominal state to update, shape (16,)
P (np.ndarray): The error state covariance to update, shape (15, 15)
z_GNSS_position (np.ndarray): The measured 3D position, shape (3,)
R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). Defaults to np.zeros(3), shape (3,).
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
Tuple[np.ndarray, np.ndarray]: Updated Tuple(x_injected, P_injected):
x_injected: The nominal state after injection of updated error state, shape (16,)
P_injected: The error state covariance after error state update and injection, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.update_GNSS: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (
15, 15), f"ESKF.update_GNSS: P shape incorrect {P.shape}"
assert z_GNSS_position.shape == (
3,
), f"ESKF.update_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}"
assert R_GNSS.shape == (
3,
3,
), f"ESKF.update_GNSS: R_GNSS shape incorrect {R_GNSS.shape}"
assert lever_arm.shape == (
3,
), f"ESKF.update_GNSS: lever_arm shape incorrect {lever_arm.shape}"
I = np.eye(*P.shape)
innovation, S = self.innovation_GNSS_position(x_nominal, P,
z_GNSS_position, R_GNSS,
lever_arm)
# H_x = np.concatenate((np.identity(3), np.zeros((3,13))), axis=1) # Eq. (10.80) Measurement matrix
# q = x_nominal[ATT_IDX]
# eta = q[0]
# epsilon = q[1:]
# Q_bottom = eta * np.eye(3) + cross_product_matrix(epsilon)
# Q_top = -epsilon.T
# Q_deltaTheta = 0.5 * np.concatenate(([Q_top], Q_bottom), axis=0)
# X_deltax = la.block_diag(np.identity(6), Q_deltaTheta, np.identity(6))
# H = H_x @ X_deltax
H = np.concatenate((np.identity(3), np.zeros((3, 12))), axis=1)
# in case of a specified lever arm
if not np.allclose(lever_arm, 0):
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX],
debug=self.debug)
H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm,
debug=self.debug)
# KF error state update
W = (la.solve(
S.T,
H @ P.T)).T # Kalman gain: W = P @ np.transpose(H) @ np.inv(S)
#delta_x = x_nominal + W @ innovation # delta x ... think this is wrong !!
delta_x = W @ innovation
Jo = I - W @ H # for Joseph form
P_update = Jo @ P @ np.transpose(Jo) + W @ R_GNSS @ np.transpose(
W) # P update
# error state injection
x_injected, P_injected = self.inject(x_nominal, delta_x, P_update)
assert x_injected.shape == (
16,
), f"ESKF.update_GNSS: x_injected shape incorrect {x_injected.shape}"
assert P_injected.shape == (
15,
15,
), f"ESKF.update_GNSS: P_injected shape incorrect {P_injected.shape}"
return x_injected, P_injected
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0,
atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion**2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
position_prediction = position + Ts * velocity + Ts**2 * 0.5 * (
R @ acceleration + self.g
) # sverre: velocity er allerede i WORLD TODO: Calculate predicted position
# TODO: Calculate predicted velocity
velocity_prediction = velocity + Ts * (
R @ acceleration + self.g
) #skal denne roteres når det antas at a = R(q)(a_m - a_b) + g?
# TODO: Calculate predicted quaternion
kappa = Ts * omega # sverre: står i hintet at denne skal oppgis i BODY
kappa_norm = np.linalg.norm(
kappa
) # sverre: dette skal være en 2-norm #############er dette riktig norm?##########
qr = np.zeros(4)
# qr[0] = np.cos(0.5*kappa_norm)
qr[0] = np.cos(0.5 * kappa_norm)
qr[1:4] = np.sin(0.5 * kappa_norm) * (kappa.T / kappa_norm)
quaternion_prediction = quaternion_product(
quaternion, qr.T
) # sverre: skal denne uttrykkes i body eller world? Er forskjellige definisjoner av q_dot i boka og i hintet til oppgaven.
# Normalize quaternion
quaternion_prediction = quaternion_prediction / np.linalg.norm(
quaternion_prediction) # TODO: Normalize
acceleration_bias_prediction = acceleration_bias - Ts * self.p_acc * acceleration_bias # TODO: Calculate predicted acceleration bias
gyroscope_bias_prediction = gyroscope_bias - Ts * self.p_gyro * gyroscope_bias # TODO: Calculate predicted gyroscope bias
x_nominal_predicted = np.concatenate((
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
))
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the ion interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0,
atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion**2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
position_prediction = position + Ts * velocity + (
(1 / 2) * Ts**2) * (R @ acceleration + self.g)
velocity_prediction = velocity + Ts * (R @ acceleration + self.g)
k = Ts * omega
k_norm_2 = la.norm(k, 2)
rhs_quat = np.array(
[np.cos(k_norm_2 / 2), *np.sin(k_norm_2 / 2) * (k.T) / k_norm_2])
rhs_quat = rhs_quat.T
# Normalize quaternion
quaternion_prediction = quaternion_product(quaternion,
rhs_quat) # TODO: Normalize
quaternion_prediction /= la.norm(quaternion_prediction, 2)
p_ab = self.p_acc
p_wb = self.p_gyro
acceleration_bias_prediction = acceleration_bias * np.exp(
-Ts * p_ab) #acceleration_bias*exp(-Ts*p_ab)
gyroscope_bias_prediction = gyroscope_bias * np.exp(-Ts * p_wb)
x_nominal_predicted = np.concatenate((
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
))
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
def innovation_GNSS_position(
self,
x_nominal: np.ndarray,
P: np.ndarray,
z_GNSS_position: np.ndarray,
R_GNSS: np.ndarray,
lever_arm: np.ndarray = np.zeros(3),
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculates the innovation and its covariance for a GNSS position measurement
Args:
x_nominal (np.ndarray): The nominal state to calculate the innovation from, shape (16,)
P (np.ndarray): The error state covariance to calculate the innovation covariance from, shape (15, 15)
z_GNSS_position (np.ndarray): The measured 3D position, shape (3,)
R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference. Defaults to np.zeros(3).
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
Tuple[ np.ndarray, np.ndarray ]: Innovation Tuple(v, S):
v: innovation, shape (3,)
S: innovation covariance, shape (3, 3)
"""
assert x_nominal.shape == (
16,
), f"ESKF.innovation_GNSS: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (
15, 15), f"ESKF.innovation_GNSS: P shape incorrect {P.shape}"
assert z_GNSS_position.shape == (
3,
), f"ESKF.innovation_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}"
assert R_GNSS.shape == (
3,
3,
), f"ESKF.innovation_GNSS: R_GNSS shape incorrect {R_GNSS.shape}"
assert lever_arm.shape == (
3,
), f"ESKF.innovation_GNSS: lever_arm shape incorrect {lever_arm.shape}"
Hx = np.zeros((3, 16)) # TODO: measurement matrix
Hx[:, 0:3] = np.eye(3)
q = x_nominal[ATT_IDX]
X = np.zeros((16, 15))
X[0:6, 0:6] = np.eye(6)
X[10:16, 9:15] = np.eye(6)
X[6, 6:9] = 0.5 * np.array([-q[1], -q[2], -q[3]])
X[7, 6:9] = 0.5 * np.array([q[0], -q[3], q[2]])
X[8, 6:9] = 0.5 * np.array([q[3], q[0], -q[1]])
X[9, 6:9] = 0.5 * np.array([-q[2], q[1], q[0]])
H = Hx @ X #sverre: 10.76
v = z_GNSS_position - Hx @ x_nominal # sverre: bruker Hx her siden den er (3,16) TODO: innovation
# H_2 = np.block([np.eye(3), np.zeros((3,12))]) # Simon: Hvorfor denne H-en?
# v_2 = z_GNSS_position - x_nominal[POS_IDX] # Simon: Hvorfor denne v-en?
# leverarm compensation
if not np.allclose(lever_arm, 0):
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX],
debug=self.debug)
H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm,
debug=self.debug)
v -= R @ lever_arm
S = H @ P @ H.T + R_GNSS # TODO: innovation covariance
assert v.shape == (
3, ), f"ESKF.innovation_GNSS: v shape incorrect {v.shape}"
assert S.shape == (
3, 3), f"ESKF.innovation_GNSS: S shape incorrect {S.shape}"
return v, S
def update_GNSS_position(
self,
x_nominal: np.ndarray,
P: np.ndarray,
z_GNSS_position: np.ndarray,
R_GNSS: np.ndarray,
lever_arm: np.ndarray = np.zeros(3),
) -> Tuple[np.ndarray, np.ndarray]:
"""Updates the state and covariance from a GNSS position measurement
Args:
x_nominal (np.ndarray): The nominal state to update, shape (16,)
P (np.ndarray): The error state covariance to update, shape (15, 15)
z_GNSS_position (np.ndarray): The measured 3D position, shape (3,)
R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). Defaults to np.zeros(3), shape (3,).
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
Tuple[np.ndarray, np.ndarray]: Updated Tuple(x_injected, P_injected):
x_injected: The nominal state after injection of updated error state, shape (16,)
P_injected: The error state covariance after error state update and injection, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.update_GNSS: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (
15, 15), f"ESKF.update_GNSS: P shape incorrect {P.shape}"
assert z_GNSS_position.shape == (
3,
), f"ESKF.update_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}"
assert R_GNSS.shape == (
3,
3,
), f"ESKF.update_GNSS: R_GNSS shape incorrect {R_GNSS.shape}"
assert lever_arm.shape == (
3,
), f"ESKF.update_GNSS: lever_arm shape incorrect {lever_arm.shape}"
I = np.eye(*P.shape)
innovation, S = self.innovation_GNSS_position(x_nominal, P,
z_GNSS_position, R_GNSS,
lever_arm)
Hx = np.zeros((3, 16)) # TODO: measurement matrix
Hx[:, 0:3] = np.eye(3)
q = x_nominal[ATT_IDX]
X = np.zeros((16, 15))
X[0:6, 0:6] = np.eye(6)
X[10:16, 9:15] = np.eye(6)
X[6, 6:9] = 0.5 * np.array([-q[1], -q[2], -q[3]])
X[7, 6:9] = 0.5 * np.array([q[0], -q[3], q[2]])
X[8, 6:9] = 0.5 * np.array([q[3], q[0], -q[1]])
X[9, 6:9] = 0.5 * np.array([-q[2], q[1], q[0]])
H = Hx @ X #sverre: 10.76
# H = np.block([np.eye(3), np.zeros((3,12))]) # Simon: Hvorfor denne H-en??
# in case of a specified lever arm
if not np.allclose(lever_arm, 0):
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX],
debug=self.debug)
H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm,
debug=self.debug)
# KF error state update
W = P @ H.T @ np.linalg.inv(S) # sverre: 10.75 TODO: Kalman gain
#delta_x = np.zeros((15,)) # TODO: delta x
delta_x = W @ innovation #sverre: delta_x (15,)
Jo = I - W @ H # for Joseph form
# TODO: P update
# P_update = Jo @ P @ Jo.T + W @ R @ W.T #sverre: 4.10 (numerically faster) (jospeh form)
P_update = Jo @ P @ Jo.T + W @ R_GNSS @ W.T #sverre: 4.10 (numerically faster) (jospeh form) # Simon: Hvorfor R_GNSS??
# error state injection
x_injected, P_injected = self.inject(x_nominal, delta_x, P_update)
assert x_injected.shape == (
16,
), f"ESKF.update_GNSS: x_injected shape incorrect {x_injected.shape}"
assert P_injected.shape == (
15,
15,
), f"ESKF.update_GNSS: P_injected shape incorrect {P_injected.shape}"
return x_injected, P_injected
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3,
), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion ** 2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
position_prediction = position + Ts*velocity + (Ts**2*(R@acceleration + self.g))/2
velocity_prediction = velocity +Ts*(R@acceleration + self.g)
vec_inc = Ts*omega
vec_inc_norm = la.norm(vec_inc)
r_quat = np.array([np.cos(vec_inc_norm/2), *(vec_inc.T * np.sin(vec_inc_norm/2))/vec_inc_norm]).T
quaternion_prediction = quaternion_product(quaternion, r_quat)
# dobbeltsjekk
# Normalize quaternion
quaternion_prediction = quaternion_prediction/la.norm(quaternion_prediction)
# acceleration_bias_prediction = acceleration_bias - Ts * self.p_acc * acceleration_bias
# gyroscope_bias_prediction = gyroscope_bias - Ts * self.p_gyro * gyroscope_bias #? Spør studass her
acceleration_bias_prediction = acceleration_bias*np.exp(-self.p_acc*Ts) # potensielt += her
gyroscope_bias_prediction = gyroscope_bias*np.exp(-self.p_gyro*Ts) # potensielt += her
x_nominal_predicted = np.concatenate(
(
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
)
)
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0,
atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion**2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
g = np.array([0, 0, 9.81])
acceleration_world = R @ acceleration + g
# omega_world = R @ omega
position_prediction = position + Ts * velocity + Ts**2 / 2 * acceleration_world
velocity_prediction = velocity + Ts * acceleration_world
kappa = Ts * omega
kappa_norm = la.norm(kappa)
delta_quat = np.array([
np.cos(kappa_norm / 2),
*(np.sin(kappa_norm / 2) * kappa.T / kappa_norm)
])
quaternion_prediction = quaternion_product(quaternion, delta_quat)
# Normalize quaternion
quaternion_prediction = quaternion_prediction / la.norm(
quaternion_prediction)
acceleration_bias_prediction = (1 -
Ts * self.p_acc) * acceleration_bias
gyroscope_bias_prediction = (1 - Ts * self.p_gyro) * gyroscope_bias
x_nominal_predicted = np.concatenate((
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
))
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0,
atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion**2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
g = np.array([0, 0, 9.81])
position_prediction = position + velocity * Ts + 1 / 2 * (
R @ acceleration + g) * Ts**2
velocity_prediction = velocity + (R @ acceleration + g) * Ts
k = omega * Ts
k_norm = la.norm(k)
e = np.array(
[np.cos(k_norm / 2), *(np.sin(k_norm / 2) * k.T / k_norm)]).T
quaternion_prediction = quaternion_product(quaternion, e)
quaternion_prediction = quaternion_prediction / la.norm(
quaternion_prediction)
#I = np.eye(3)
#acceleration_bias_prediction = acceleration_bias - self.p_acc * I @ acceleration_bias * Ts
#gyroscope_bias_prediction = gyroscope_bias - self.p_gyro * I @ gyroscope_bias * Ts
acceleration_bias_prediction = acceleration_bias * np.exp(
-self.p_acc * Ts)
gyroscope_bias_prediction = gyroscope_bias * np.exp(-self.p_gyro * Ts)
x_nominal_predicted = np.concatenate((
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
))
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
def update_GNSS_position(
self,
x_nominal: np.ndarray,
P: np.ndarray,
z_GNSS_position: np.ndarray,
R_GNSS: np.ndarray,
lever_arm: np.ndarray = np.zeros(3),
) -> Tuple[np.ndarray, np.ndarray]:
"""Updates the state and covariance from a GNSS position measurement
Args:
x_nominal (np.ndarray): The nominal state to update, shape (16,)
P (np.ndarray): The error state covariance to update, shape (15, 15)
z_GNSS_position (np.ndarray): The measured 3D position, shape (3,)
R_GNSS (np.ndarray): The estimated covariance matrix of the measurement, shape (3, 3)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). Defaults to np.zeros(3), shape (3,).
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
Tuple[np.ndarray, np.ndarray]: Updated Tuple(x_injected, P_injected):
x_injected: The nominal state after injection of updated error state, shape (16,)
P_injected: The error state covariance after error state update and injection, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.update_GNSS: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (
15, 15), f"ESKF.update_GNSS: P shape incorrect {P.shape}"
assert z_GNSS_position.shape == (
3,
), f"ESKF.update_GNSS: z_GNSS_position shape incorrect {z_GNSS_position.shape}"
assert R_GNSS.shape == (
3,
3,
), f"ESKF.update_GNSS: R_GNSS shape incorrect {R_GNSS.shape}"
assert lever_arm.shape == (
3,
), f"ESKF.update_GNSS: lever_arm shape incorrect {lever_arm.shape}"
I = np.eye(*P.shape)
innovation, S = self.innovation_GNSS_position(x_nominal, P,
z_GNSS_position, R_GNSS,
lever_arm)
# TODO: measurement matrix
H = np.zeros((3, 15))
H[POS_IDX * POS_IDX] = np.eye(3)
# in case of a specified lever arm
if not np.allclose(lever_arm, 0):
R = quaternion_to_rotation_matrix(x_nominal[ATT_IDX],
debug=self.debug)
H[:, ERR_ATT_IDX] = -R @ cross_product_matrix(lever_arm,
debug=self.debug)
# KF error state update
# TODO: Kalman gain
W = P @ H.T @ la.inv(S)
# TODO: delta x
delta_x = W @ innovation
Jo = I - W @ H # for Joseph form
# TODO: P update
P_update = Jo @ P @ Jo.T + W @ R_GNSS @ W.T
# error state injection
x_injected, P_injected = self.inject(x_nominal, delta_x, P_update)
assert x_injected.shape == (
16,
), f"ESKF.update_GNSS: x_injected shape incorrect {x_injected.shape}"
assert P_injected.shape == (
15,
15,
), f"ESKF.update_GNSS: P_injected shape incorrect {P_injected.shape}"
return x_injected, P_injected
def predict_nominal(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> np.ndarray:
"""Discrete time prediction, equation (10.58)
Args:
x_nominal (np.ndarray): The nominal state to predict, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the predicted interval, shape (3,)
omega (np.ndarray): The estimated rotation rate in body for the prediction interval, shape (3,)
Ts (float): The sampling time
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: The predicted nominal state, shape (16,)
"""
assert x_nominal.shape == (
16,
), f"ESKF.predict_nominal: x_nominal incorrect shape {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.predict_nominal: acceleration incorrect shape {acceleration.shape}"
assert omega.shape == (
3, ), f"ESKF.predict_nominal: omega incorrect shape {omega.shape}"
# Extract states
position = x_nominal[POS_IDX]
velocity = x_nominal[VEL_IDX]
quaternion = x_nominal[ATT_IDX]
acceleration_bias = x_nominal[ACC_BIAS_IDX]
gyroscope_bias = x_nominal[GYRO_BIAS_IDX]
if self.debug:
assert np.allclose(
np.linalg.norm(quaternion), 1, rtol=0,
atol=1e-15), "ESKF.predict_nominal: Quaternion not normalized."
assert np.allclose(
np.sum(quaternion**2), 1, rtol=0, atol=1e-15
), "ESKF.predict_nominal: Quaternion not normalized and norm failed to catch it."
R = quaternion_to_rotation_matrix(quaternion, debug=self.debug)
# Assumptions
a = R @ acceleration + self.g
w = omega
# Update predictions for new time steps using the hint
position_prediction = position + Ts * velocity + Ts**2 * a / 2
velocity_prediction = velocity + Ts * a
# Update quaternion using the hint
kappa = Ts * w
kappa_norm = np.linalg.norm(kappa)
exponent = np.array([
np.cos(kappa_norm / 2),
*(np.sin(kappa_norm / 2) * (kappa.T) / kappa_norm)
]).T
quaternion_prediction = quaternion_product(quaternion, exponent)
# Normalize quaternion
quaternion_prediction = quaternion_prediction / np.linalg.norm(
quaternion_prediction)
# Update biases
acceleration_bias_prediction = acceleration_bias * np.exp(
-self.p_acc * Ts)
gyroscope_bias_prediction = gyroscope_bias * np.exp(-self.p_gyro * Ts)
x_nominal_predicted = np.concatenate((
position_prediction,
velocity_prediction,
quaternion_prediction,
acceleration_bias_prediction,
gyroscope_bias_prediction,
))
assert x_nominal_predicted.shape == (
16,
), f"ESKF.predict_nominal: x_nominal_predicted shape incorrect {x_nominal_predicted.shape}"
return x_nominal_predicted
