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 * CatSlice(start=0, stop=3)] = -1 * R
G[ERR_ATT_IDX * CatSlice(start=3, stop=6)] = -1 * np.eye(3)
G[ERR_ACC_BIAS_IDX * CatSlice(start=6, stop=9)] = np.eye(3)
G[ERR_GYRO_BIAS_IDX * CatSlice(start=9, stop=12)] = np.eye(3)
assert G.shape == (
15, 12), f"ESKF.Gerr: G-matrix shape incorrect {G.shape}"
return G
def discrete_error_matrices(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculate the discrete time linearized error state transition and covariance matrix.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the prediction 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:
Tuple[np.ndarray, np.ndarray]: Discrete error matrices Tuple(Ad, GQGd)
Ad: The discrete time error state system matrix, shape (15, 15)
GQGd: The discrete time noise covariance matrix, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.discrete_error_matrices: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.discrete_error_matrices: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3,
), f"ESKF.discrete_error_matrices: omega shape incorrect {omega.shape}"
A = self.Aerr(x_nominal, acceleration, omega)
G = self.Gerr(x_nominal)
D = self.Q_err
V = np.block([[-A, G@[email protected]],[np.zeros((15,15)), A.T]]) * Ts
assert V.shape == (
30,
30,
), f"ESKF.discrete_error_matrices: Van Loan matrix shape incorrect {omega.shape}"
# VanLoanMatrix = la.expm(V) # This can be slow...
VanLoanMatrix = np.eye(30) + V + V @ V / 2 # Second order taylor approximation instead of la.expm
Ad = VanLoanMatrix[CatSlice(15,30)**2].T #V1 transposed
GQGd = Ad @ VanLoanMatrix[CatSlice(0,15)*CatSlice(15,30)] # V1.T*V2
assert Ad.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: Ad-matrix shape incorrect {Ad.shape}"
assert GQGd.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: GQGd-matrix shape incorrect {GQGd.shape}"
return Ad, GQGd
def NEESes(
cls,
x_nominal: np.ndarray,
P: np.ndarray,
x_true: np.ndarray,
) -> np.ndarray:
"""Calculates the total NEES and the NEES for the substates
Args:
x_nominal (np.ndarray): The nominal estimate
P (np.ndarray): The error state covariance
x_true (np.ndarray): The true state
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: NEES for [all, position, velocity, attitude, acceleration_bias, gyroscope_bias], shape (6,)
"""
assert x_nominal.shape == (
16, ), f"ESKF.NEES: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (15, 15), f"ESKF.NEES: P shape incorrect {P.shape}"
assert x_true.shape == (
16, ), f"ESKF.NEES: x_true shape incorrect {x_true.shape}"
ATT_IDX_THETA = CatSlice(start=6, stop=9)
ACC_BIAS_IDX_THETA = CatSlice(start=9, stop=12)
GYRO_BIAS_IDX_THETA = CatSlice(start=12, stop=15)
d_x = cls.delta_x(x_nominal, x_true)
NEES_all = d_x.T @ np.linalg.inv(P) @ d_x # TODO: NEES all
NEES_pos = d_x[POS_IDX].T @ np.linalg.inv(
P[POS_IDX * POS_IDX]) @ d_x[POS_IDX] # TODO: NEES position
NEES_vel = d_x[VEL_IDX].T @ np.linalg.inv(
P[VEL_IDX * VEL_IDX]) @ d_x[VEL_IDX] # TODO: NEES velocity
NEES_att = d_x[ATT_IDX_THETA].T @ np.linalg.inv(
P[ATT_IDX_THETA * ATT_IDX_THETA]
) @ d_x[
ATT_IDX_THETA] # TODO: NEES attitude sverre: det skal vel være theta og ikke q NEES baserer seg på?
NEES_accbias = d_x[ACC_BIAS_IDX_THETA].T @ np.linalg.inv(
P[ACC_BIAS_IDX_THETA * ACC_BIAS_IDX_THETA]) @ d_x[
ACC_BIAS_IDX_THETA] # TODO: NEES accelerometer bias
NEES_gyrobias = d_x[GYRO_BIAS_IDX_THETA].T @ np.linalg.inv(
P[GYRO_BIAS_IDX_THETA * GYRO_BIAS_IDX_THETA]) @ d_x[
GYRO_BIAS_IDX_THETA] # TODO: NEES gyroscope bias
# NEES_att = d_x[ERR_ATT_IDX].T @ np.linalg.inv(P[ERR_ATT_IDX * ERR_ATT_IDX]) @ d_x[ERR_ATT_IDX] # TODO: NEES attitude sverre: det skal vel være theta og ikke q NEES baserer seg på?
# NEES_accbias = d_x[ERR_ATT_IDX].T @ np.linalg.inv(P[ERR_ATT_IDX * ERR_ATT_IDX]) @ d_x[ERR_ATT_IDX] # TODO: NEES accelerometer bias
# NEES_gyrobias = d_x[ERR_GYRO_BIAS_IDX].T @ np.linalg.inv(P[ERR_GYRO_BIAS_IDX * ERR_GYRO_BIAS_IDX]) @ d_x[ERR_GYRO_BIAS_IDX] # TODO: NEES gyroscope bias
NEESes = np.array([
NEES_all, NEES_pos, NEES_vel, NEES_att, NEES_accbias, NEES_gyrobias
])
assert np.all(NEESes >= 0), "ESKF.NEES: one or more negative NEESes"
return NEESes
def alternative_NEESes(
cls,
x_nominal: np.ndarray,
P: np.ndarray,
x_true: np.ndarray,
) -> np.ndarray:
"""
Calculates some alternativ NEESes for the substates
Args:
x_nominal (np.ndarray): The nominal estimate
P (np.ndarray): The error state covariance
x_true (np.ndarray): The true state
Returns:
np.ndarray: NEES for [[position, velocity, attitude],[position, velocity],
[acceleration_bias, gyroscope_bias]], shape (3,)
"""
assert x_nominal.shape == (
16, ), f"ESKF.NEES: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (15, 15), f"ESKF.NEES: P shape incorrect {P.shape}"
assert x_true.shape == (
16, ), f"ESKF.NEES: x_true shape incorrect {x_true.shape}"
d_x = cls.delta_x(x_nominal, x_true)
POS_VEL_ERR_ATT_IDX = CatSlice(0, stop=9)
POS_VEL_IDX = CatSlice(0, stop=6)
ERR_ACC_GYRO_BIAS_IDX = CatSlice(9, stop=15)
NEES_pos_vel_att = cls._NEES(P[POS_VEL_ERR_ATT_IDX**2],
d_x[POS_VEL_ERR_ATT_IDX])
NEES_pos_vel = cls._NEES(P[POS_VEL_IDX**2], d_x[POS_VEL_IDX])
P_pos_att = la.block_diag(P[POS_IDX**2], P[ERR_ATT_IDX**2])
d_x_pos_att = np.concatenate((d_x[POS_IDX], d_x[ERR_ATT_IDX]))
NEES_pos_att = cls._NEES(P_pos_att, d_x_pos_att)
P_vel_att = la.block_diag(P[VEL_IDX**2], P[ERR_ATT_IDX**2])
d_x_vel_att = np.concatenate((d_x[VEL_IDX], d_x[ERR_ATT_IDX]))
NEES_vel_att = cls._NEES(P_vel_att, d_x_vel_att)
NEES_accbias_gyrobias = cls._NEES(P[ERR_ACC_GYRO_BIAS_IDX**2],
d_x[ERR_ACC_GYRO_BIAS_IDX])
NEESes = np.array([
NEES_pos_vel_att, NEES_pos_vel, NEES_pos_att, NEES_vel_att,
NEES_accbias_gyrobias
])
assert np.all(NEESes >= 0), "ESKF.NEES: one or more negative NEESes"
return NEESes
def Fu(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
"""Calculate the Jacobian of f with respect to u.
Parameters
----------
x : np.ndarray, shape=(3,)
the robot state
u : np.ndarray, shape=(3,)
the odometry
Returns
-------
np.ndarray
The Jacobian of f wrt. u.
"""
Fu = np.eye(3) # TODO, eq (11.14)
Fu[CatSlice(start=0, stop=2) * CatSlice(start=0, stop=2)] = np.array(
[[np.cos(x[2]), -np.sin(x[2])], [np.sin(x[2]),
np.cos(x[2])]])
# assert Fu.shape == (3, 3), "EKFSLAM.Fu: wrong shape"
return Fu
def H(self, x_nominal: np.ndarray) -> np.ndarray:
# Implements Eq. (10.78) :
eta, *eps = x_nominal[ATT_IDX]
eps = np.array(eps)
Q_dtheta = 1/2. * np.array([
-1 * eps,
[eta, -1*eps[2], eps[1]],
[eps[2], eta, -1*eps[0]],
[-1*eps[1], eps[0], eta]
])
# Implements Eq. (10.77) :
X_dx = np.zeros((16, 15))
X_dx[CatSlice(start=0, stop=6)**2] = np.eye(6)
X_dx[CatSlice(start=6, stop=6+4) *
CatSlice(start=6, stop=6+3)] = Q_dtheta
X_dx[CatSlice(start=10, stop=16) *
CatSlice(start=9, stop=15)] = np.eye(6)
# Implements (10.76) :
H = self.H_x @ X_dx
return H
def __post_init__(self):
if self.debug:
print(
"ESKF in debug mode, some numeric properties are checked at the expense of calculation speed"
)
self.Q_err = (
la.block_diag(
self.sigma_acc * np.eye(3),
self.sigma_gyro * np.eye(3),
self.sigma_acc_bias * np.eye(3),
self.sigma_gyro_bias * np.eye(3),
)
** 2
)
# Measurement matrix:
# Implements Eq. (10.80)
self.H_x = np.zeros((3, 16))
self.H_x[CatSlice(start=0, stop=3) * POS_IDX] = np.eye(3)
def discrete_error_matrices(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculate the discrete time linearized error state transition and covariance matrix.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the prediction 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:
Tuple[np.ndarray, np.ndarray]: Discrete error matrices Tuple(Ad, GQGd)
Ad: The discrete time error state system matrix, shape (15, 15)
GQGd: The discrete time noise covariance matrix, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.discrete_error_matrices: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.discrete_error_matrices: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3,
), f"ESKF.discrete_error_matrices: omega shape incorrect {omega.shape}"
A = self.Aerr(x_nominal, acceleration, omega)
G = self.Gerr(x_nominal)
V = np.zeros((30, 30))
V[CatSlice(start=0, stop=15) * CatSlice(start=0, stop=15)] = -A
V[CatSlice(start=0, stop=15) *
CatSlice(start=15, stop=30)] = G @ self.Q_err @ np.transpose(G)
V[CatSlice(start=15, stop=30) *
CatSlice(start=15, stop=30)] = np.transpose(A)
assert V.shape == (
30,
30,
), f"ESKF.discrete_error_matrices: Van Loan matrix shape incorrect {omega.shape}"
VanLoanMatrix = la.expm(
Ts * V
) # This can be slow... if too slow use Taylor or Pade approximation
V1 = VanLoanMatrix[CatSlice(start=15, stop=30) *
CatSlice(start=15, stop=30)]
V2 = VanLoanMatrix[CatSlice(start=0, stop=15) *
CatSlice(start=15, stop=30)]
Ad = np.transpose(V1)
GQGd = np.transpose(V1) @ V2
assert Ad.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: Ad-matrix shape incorrect {Ad.shape}"
assert GQGd.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: GQGd-matrix shape incorrect {GQGd.shape}"
return Ad, GQGd
import numpy as np
import scipy.linalg as la
from quaternion import (
euler_to_quaternion,
quaternion_product,
quaternion_to_euler,
quaternion_to_rotation_matrix,
)
# from state import NominalIndex, ErrorIndex
from utils import cross_product_matrix
# %% indices
POS_IDX = CatSlice(start=0, stop=3)
VEL_IDX = CatSlice(start=3, stop=6)
ATT_IDX = CatSlice(start=6, stop=10)
ACC_BIAS_IDX = CatSlice(start=10, stop=13)
GYRO_BIAS_IDX = CatSlice(start=13, stop=16)
ERR_ATT_IDX = CatSlice(start=6, stop=9)
ERR_ACC_BIAS_IDX = CatSlice(start=9, stop=12)
ERR_GYRO_BIAS_IDX = CatSlice(start=12, stop=15)
# %% The class
@dataclass
class ESKF:
sigma_acc: float
sigma_gyro: float
def discrete_error_matrices(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculate the discrete time linearized error state transition and covariance matrix.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the prediction 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:
Tuple[np.ndarray, np.ndarray]: Discrete error matrices Tuple(Ad, GQGd)
Ad: The discrete time error state system matrix, shape (15, 15)
GQGd: The discrete time noise covariance matrix, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.discrete_error_matrices: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.discrete_error_matrices: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3,
), f"ESKF.discrete_error_matrices: omega shape incorrect {omega.shape}"
A = self.Aerr(x_nominal, acceleration, omega)
G = self.Gerr(x_nominal)
FIRST_HALF_INDEX = CatSlice(start=0, stop=15)
SECOND_HALF_INDEX = CatSlice(start=15, stop=30)
V = np.zeros((30, 30))
V[FIRST_HALF_INDEX * FIRST_HALF_INDEX] = -A
V[FIRST_HALF_INDEX *
SECOND_HALF_INDEX] = G @ self.Q_err @ G.T # sverre: 10.69
V[SECOND_HALF_INDEX * SECOND_HALF_INDEX] = A.T
# V2 = np.block([[-A, [email protected][email protected]], [np.zeros_like(A), A.T]]) #* Ts
assert V.shape == (
30,
30,
), f"ESKF.discrete_error_matrices: Van Loan matrix shape incorrect {omega.shape}"
# VanLoanMatrix = la.expm(V*Ts) # This can be slow...
VanLoanMatrix = (np.eye(30) + V * Ts + V @ V / 2 * Ts**2)
Ad = VanLoanMatrix[SECOND_HALF_INDEX * SECOND_HALF_INDEX].T
GQGd = VanLoanMatrix[FIRST_HALF_INDEX * SECOND_HALF_INDEX]
# Ad = VanLoanMatrix[CatSlice(15,30)**2].T
# GQGd = Ad @ VanLoanMatrix[CatSlice(0,15)*CatSlice(15,30)]
assert Ad.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: Ad-matrix shape incorrect {Ad.shape}"
assert GQGd.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: GQGd-matrix shape incorrect {GQGd.shape}"
return Ad, GQGd
def NEESes(
cls,
x_nominal: np.ndarray,
P: np.ndarray,
x_true: np.ndarray,
) -> np.ndarray:
"""Calculates the total NEES and the NEES for the substates
Args:
x_nominal (np.ndarray): The nominal estimate
P (np.ndarray): The error state covariance
x_true (np.ndarray): The true state
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: NEES for [all, position, velocity, attitude, acceleration_bias, gyroscope_bias], shape (6,)
"""
assert x_nominal.shape == (
16, ), f"ESKF.NEES: x_nominal shape incorrect {x_nominal.shape}"
assert P.shape == (15, 15), f"ESKF.NEES: P shape incorrect {P.shape}"
assert x_true.shape == (
16, ), f"ESKF.NEES: x_true shape incorrect {x_true.shape}"
d_x = cls.delta_x(x_nominal, x_true)
NEES_all = cls._NEES(d_x, P) # TODO: NEES all
NEES_pos = cls._NEES(d_x[POS_IDX],
P[POS_IDX**2]) # TODO: NEES position
NEES_vel = cls._NEES(d_x[VEL_IDX],
P[VEL_IDX**2]) # TODO: NEES velocity
NEES_att = cls._NEES(d_x[ERR_ATT_IDX],
P[ERR_ATT_IDX**2]) # TODO: NEES attitude
NEES_accbias = cls._NEES(
d_x[ERR_ACC_BIAS_IDX],
P[ERR_ACC_BIAS_IDX**2]) # TODO: NEES accelerometer bias
NEES_gyrobias = cls._NEES(
d_x[ERR_GYRO_BIAS_IDX],
P[ERR_GYRO_BIAS_IDX**2]) # TODO: NEES gyroscope bias
NEES_pos_x = cls._NEES(d_x[CatSlice(start=0, stop=1)],
P[CatSlice(start=0, stop=1)**2])
NEES_pos_y = cls._NEES(d_x[CatSlice(start=1, stop=2)],
P[CatSlice(start=1, stop=2)**2])
NEES_pos_z = cls._NEES(d_x[CatSlice(start=2, stop=3)],
P[CatSlice(start=2, stop=3)**2])
NEES_gyrobias_x = cls._NEES(d_x[CatSlice(start=12, stop=13)],
P[CatSlice(start=12, stop=13)**2])
NEES_gyrobias_y = cls._NEES(d_x[CatSlice(start=13, stop=14)],
P[CatSlice(start=13, stop=14)**2])
NEES_gyrobias_z = cls._NEES(d_x[CatSlice(start=14, stop=15)],
P[CatSlice(start=14, stop=15)**2])
NEES_accbias_x = cls._NEES(d_x[CatSlice(start=9, stop=10)],
P[CatSlice(start=9, stop=10)**2])
NEES_accbias_y = cls._NEES(d_x[CatSlice(start=10, stop=11)],
P[CatSlice(start=10, stop=11)**2])
NEES_accbias_z = cls._NEES(d_x[CatSlice(start=11, stop=12)],
P[CatSlice(start=11, stop=12)**2])
NEESes = np.array([
NEES_all,
NEES_pos,
NEES_vel,
NEES_att,
NEES_accbias,
NEES_gyrobias,
NEES_pos_x,
NEES_pos_y,
NEES_pos_z,
NEES_gyrobias_x,
NEES_gyrobias_y,
NEES_gyrobias_z,
NEES_accbias_x,
NEES_accbias_y,
NEES_accbias_z,
])
assert np.all(NEESes >= 0), "ESKF.NEES: one or more negative NEESes"
return NEESes
def NIS_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),
) -> float:
"""Calculates the NIS 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,)
lever_arm (np.ndarray, optional): The position of the GNSS receiver from the IMU reference, shape (3,). 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:
float: The normalized innovations squared (NIS)
"""
assert x_nominal.shape == (
16, ), "ESKF.NIS_GNSS: x_nominal shape incorrect " + str(
x_nominal.shape)
assert P.shape == (
15, 15), "ESKF.NIS_GNSS: P shape incorrect " + str(P.shape)
assert z_GNSS_position.shape == (
3, ), "ESKF.NIS_GNSS: z_GNSS_position shape incorrect " + str(
z_GNSS_position.shape)
assert R_GNSS.shape == (
3, 3), "ESKF.NIS_GNSS: R_GNSS shape incorrect " + str(R_GNSS.shape)
assert lever_arm.shape == (
3, ), "ESKF.NIS_GNSS: lever_arm shape incorrect " + str(
lever_arm.shape)
v, S = self.innovation_GNSS_position(x_nominal, P, z_GNSS_position,
R_GNSS, lever_arm)
# TODO: Calculate NIS
NIS = v.T @ la.inv(S) @ v
NIS_x = v[CatSlice(start=0, stop=1)].T @ la.solve(
S[CatSlice(start=0, stop=1)**2], v[CatSlice(start=0, stop=1)])
NIS_y = v[CatSlice(start=1, stop=2)].T @ la.solve(
S[CatSlice(start=1, stop=2)**2], v[CatSlice(start=1, stop=2)])
NIS_z = v[CatSlice(start=2, stop=3)].T @ la.solve(
S[CatSlice(start=2, stop=3)**2], v[CatSlice(start=2, stop=3)])
NIS_xy = v[CatSlice(start=0, stop=2)].T @ la.solve(
S[CatSlice(start=0, stop=2)**2], v[CatSlice(start=0, stop=2)])
assert NIS >= 0, "EKSF.NIS_GNSS_positionNIS: NIS not positive"
return NIS, NIS_x, NIS_y, NIS_z, NIS_xy
def discrete_error_matrices(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculate the discrete time linearized error state transition and covariance matrix.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the prediction 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:
Tuple[np.ndarray, np.ndarray]: Discrete error matrices Tuple(Ad, GQGd)
Ad: The discrete time error state system matrix, shape (15, 15)
GQGd: The discrete time noise covariance matrix, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.discrete_error_matrices: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.discrete_error_matrices: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3,
), f"ESKF.discrete_error_matrices: omega shape incorrect {omega.shape}"
A = self.Aerr(x_nominal, acceleration, omega)
G = self.Gerr(x_nominal)
V = np.zeros((30, 30))
V[CatSlice(start=0, stop=15) *
CatSlice(start=0, stop=30)] = np.concatenate(
(-A, G @ self.Q_err @ G.T), axis=1)
V[CatSlice(start=15, stop=30) *
CatSlice(start=0, stop=30)] = np.concatenate((np.zeros(
(15, 15)), A.T),
axis=1)
V = V * Ts
V1 = np.vstack([
np.hstack([-A, G @ self.Q_err @ G.T]),
np.hstack([np.zeros((15, 15)), A.T])
]) * Ts
assert np.allclose(V, V1), "V and V_1 not close..."
assert V.shape == (
30,
30,
), f"ESKF.discrete_error_matrices: Van Loan matrix shape incorrect {V.shape}"
VanLoanMatrix = la.expm(V) # This can be slow...
Ad = VanLoanMatrix[CatSlice(start=15, stop=30) *
CatSlice(start=15, stop=30)].T
GQGd = Ad * VanLoanMatrix[CatSlice(start=0, stop=15) *
CatSlice(start=15, stop=30)]
assert Ad.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: Ad-matrix shape incorrect {Ad.shape}"
assert GQGd.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: GQGd-matrix shape incorrect {GQGd.shape}"
return Ad, GQGd
def discrete_error_matrices(
self,
x_nominal: np.ndarray,
acceleration: np.ndarray,
omega: np.ndarray,
Ts: float,
) -> Tuple[np.ndarray, np.ndarray]:
"""Calculate the discrete time linearized error state transition and covariance matrix.
Args:
x_nominal (np.ndarray): The nominal state, shape (16,)
acceleration (np.ndarray): The estimated acceleration in body for the prediction 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:
Tuple[np.ndarray, np.ndarray]: Discrete error matrices Tuple(Ad, GQGd)
Ad: The discrete time error state system matrix, shape (15, 15)
GQGd: The discrete time noise covariance matrix, shape (15, 15)
"""
assert x_nominal.shape == (
16,
), f"ESKF.discrete_error_matrices: x_nominal shape incorrect {x_nominal.shape}"
assert acceleration.shape == (
3,
), f"ESKF.discrete_error_matrices: acceleration shape incorrect {acceleration.shape}"
assert omega.shape == (
3,
), f"ESKF.discrete_error_matrices: omega shape incorrect {omega.shape}"
UP_LEFT = CatSlice(0, 15)**2
UP_RIGHT = CatSlice(0, 15) * CatSlice(15, 30)
DOWN_RIGHT = CatSlice(15, 30)**2
A = self.Aerr(x_nominal, acceleration, omega)
G = self.Gerr(x_nominal)
Q = self.Q_err
V = np.zeros((30, 30))
V[UP_LEFT] = -A * Ts
V[UP_RIGHT] = G @ Q @ G.T * Ts
V[DOWN_RIGHT] = A.T * Ts
assert V.shape == (
30,
30,
), f"ESKF.discrete_error_matrices: Van Loan matrix shape incorrect {omega.shape}"
VanLoanMatrix = np.zeros(V.shape)
VanLoanMatrix = np.eye(30) + V + V @ V * 0.5
Ad = VanLoanMatrix[DOWN_RIGHT].T
GQGd = Ad @ VanLoanMatrix[UP_RIGHT]
assert Ad.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: Ad-matrix shape incorrect {Ad.shape}"
assert GQGd.shape == (
15,
15,
), f"ESKF.discrete_error_matrices: GQGd-matrix shape incorrect {GQGd.shape}"
return Ad, GQGd
