Theory

This section presents the error-state multiplicative EKF implemented on the code.

Please note that the documentation currently is bare bones, and the notation used might not be very rigorous; I will be adding more details and improving the documentation as the project progresses.

Notation

Main Variables and Parameters

Code	Math	Description
q	\(\mathbf{q}\)	Attitude quaternion
B	\(\boldsymbol{\beta}\)	Gyro bias vector
Z	\(\boldsymbol{\delta\theta}\)	Attitude error rotation vector
w	\(\boldsymbol{\omega}\)	Angular velocity vector
P	\(P\)	Error covariance matrix
Phi	\(\Phi\)	State transition matrix
Q	\(Q\)	Discrete-time process noise covariance matrix
K	\(K\)	Kalman gain matrix
H	\(H\)	Measurement matrix
R	\(R\)	Measurement noise covariance matrix
S	\(S\)	Innovation covariance matrix
sigma_v	\(\sigma_v\)	Gyro angle random walk coefficient [rad/s^{0.5}]
sigma_u	\(\sigma_u\)	Gyro rate random walk coefficient [rad/s^{1.5}]
sigma_startracker	\(\sigma_{st}\)	Star tracker measurement accuracy [arcsec]
dt	\(\Delta t\)	Base time step for ground truth propagation
dt_g	\(\Delta t_g\)	Time step between gyro measurements
I3	\(I_3\)	Identity matrix (3x3)

Subscripts and Notation Conventions

Code	Math	Description
X_h	\(\hat{X}\)	Estimated value of variable
X_t	\(X_t\)	Ground truth value of variable
X_m	\(X_m\)	Measured value of variable
X_0	\(X_0\)	Initial value of variable
X_l	\(X_l\)	Logged variable for analysis
X_d	\(\delta X\)	Error between truth and estimate (X_t - X_h)
X_n	\(X_n\)	Noise term added to variable

State and Error Representation

The nominal state consists of the estimates for the attitude quaternion and gyro bias, where \((\hat{\mathbf{q}}, \hat{\beta}) \in \mathbb{S}^3 \times \mathbb{R}^3\). However, the Kalman filter updates the error states, then injects these updates to the nominal state. The error quaternion is the relation between the ground truth quaternion, represented multiplicatevely:

\[\begin{split}\delta \mathbf{q} = \mathbf{q}_t \otimes \hat{\mathbf{q}}^{-1} = \begin{bmatrix} \boldsymbol{\delta\epsilon} \\ \delta\eta \end{bmatrix}\end{split}\]

We use the error rotation vector \(\boldsymbol{\delta\theta} \in \mathbb{R}^3\) in our state instead of the error quaternion:

\[\boldsymbol{\delta\theta} = 2 \arctan2 \left( \|\boldsymbol{\delta\epsilon}\|, \delta\eta \right) \frac{\boldsymbol{\delta\epsilon}}{\|\boldsymbol{\delta\epsilon}\|}\]

For small angles,

\[\begin{split}\delta \mathbf{q} \approx \begin{bmatrix} \frac{1}{2} \boldsymbol{\delta\theta} \\ 1 \end{bmatrix}\end{split}\]

A gyro measurement is modelled with a moving bias and white noise (random walk):

\[\boldsymbol{\omega}_m(t) = \boldsymbol{\omega}_t(t) + \boldsymbol{\beta}_t(t) + \mathbf{v}(t)\]

\[\mathbf{v}(t) \sim \mathcal{N}\!\left(0, \sigma_v^2 I_3 \right)\]

The bias grows as a Weiner process, at a level related to the rate random walk coefficient:

\[\boldsymbol{\dot{\beta_t}}(t) = \boldsymbol{u}(t)\]

\[\boldsymbol{u}(t) \sim \mathcal{N}\!\left(0, \sigma_u^2 I_3 \right)\]

The gyro bias error is defined as:

\[\boldsymbol{\delta \beta} = \boldsymbol{\beta}_t - \boldsymbol{\hat{\beta}}\]

Quaternion math

Throughout the documentation, we use the following quaternion multiplication convention. First we define the quaternion function \(\Xi\):

\[\begin{split}\Xi(\mathbf{q}) = \begin{bmatrix} q_w & -q_z & q_y \\ q_z & q_w & -q_x \\ -q_y & q_x & q_w \\ -q_x & -q_y & -q_z \end{bmatrix}\end{split}\]

Then, the quaternion multiplication is defined as:

\[\mathbf{q}_1 \otimes \mathbf{q}_2 = \begin{bmatrix} \Xi(\mathbf{q}_2) & \mathbf{q}_2 \end{bmatrix} \mathbf{q}_1\]

In the code we use the function quat_mul in utils.py to perform this multiplication. The quaternion inverse is defined as:

\[\begin{split}\mathbf{q}^{-1} = \begin{bmatrix} -q_x \\ -q_y \\ -q_z \\ q_w \end{bmatrix}\end{split}\]

Initialization

The user provides an initial gyro bias \(\boldsymbol{\beta_0}\) and initial attitude quaternion \(\boldsymbol{\mathbf{q}_0}\). Additionally, the user provides initial values for the Kalman filter estimator: the attitude error covariance \(P_q\) and the gyro bias error covariance \(P_b\); the initial estimate of the attitude error \(\boldsymbol{\delta\theta_0}\) and the initial estimate of the gyro bias error \(\boldsymbol{\delta\beta_0}\).

Ground truth update

The user inputs a desired ground truth angular velocity \(\boldsymbol{\omega_t}(t)\) with the function w_t_fun in efk.py. The user also inputs an initial gyro bias \(\boldsymbol{\beta_0}\). The ground truth quaternion and gyro bias are propagated at a period of \(\Delta t\).

Define \(\varphi = \|\boldsymbol{\omega}_t\| \Delta t\). The quaternion increment associated with the rotation \(\boldsymbol{\omega}_t \Delta t\) is:

\[\begin{split}\Delta \mathbf{q} = \begin{bmatrix} \mathbf{e} \sin(\frac{\varphi}{2}) \\ \cos(\frac{\varphi}{2}) \end{bmatrix}\end{split}\]

\[\mathbf{e} = \frac{\boldsymbol{\omega}_t}{ \|\boldsymbol{\omega}_t\| }\]

This assumes that the angular velocity is constant throughout this timestep. The ground truth update is:

\[\mathbf{q} \leftarrow \Delta \mathbf{q} \otimes \mathbf{q}\]

In a discrete step, the bias is updated as:

\[ \begin{align}\begin{aligned} \boldsymbol{\beta_t} \leftarrow \boldsymbol{\beta_t} + \boldsymbol{u_\Delta}\\\mathbf{u_\Delta} \sim \mathcal{N}\!\left(0, \sigma_u^2 \Delta t I_3 \right)\end{aligned}\end{align} \]

Estimate propagation (gyro measurements)

Gyros are used for dynamic model replacement, i.e. I do not integrate the Euler rigid body equations. Instead, I use the gyro measurements to propagate the estimate.

At gyro sampling instants separated by \(\Delta t_g\), the gyro provides a measurement which is synthesized from the ground truth angular velocity \(\boldsymbol{\omega}_t\) and the gyro bias \(\boldsymbol{\beta}_t\).

\[\boldsymbol{\omega}_m = \boldsymbol{\omega}_t + \boldsymbol{\beta}_t + \mathbf{v}_\Delta\]

\[\mathbf{v}_\Delta \sim \mathcal{N}\!\left(0, \frac{\sigma_v^2}{\Delta t_g} I_3 \right)\]

The estimate of the angular velocity is:

\[\boldsymbol{\hat{\omega}} = \boldsymbol{\omega}_m - \hat{\boldsymbol{\beta}}\]

We propagate the estimated attitude quaternion the same way as the ground truth (see the section above).

The bias is constant in propagation:

\[\hat{\boldsymbol{\beta}} \leftarrow \hat{\boldsymbol{\beta}}\]

The covariance propagation in a gyro measurement step is discussed in the next two sections.

Linearized Error-State Propagation

Let \(\boldsymbol{\delta x} = \begin{bmatrix} \boldsymbol{\delta\theta} \\ \delta\boldsymbol{\beta} \end{bmatrix} \in \mathbb{R}^6\). For a step \(\Delta t_g\) with input \(\boldsymbol{\hat{\omega}}\), the first-order discrete transition is:

\[\boldsymbol{\delta x} \leftarrow \Phi \boldsymbol{\delta x}\]

\[\begin{split}\Phi = \begin{bmatrix} \Phi_{11} & \Phi_{12} \\ 0 & I_3 \end{bmatrix}\end{split}\]

With \(\varphi = \|\boldsymbol{\hat{\omega}}\| \Delta t_g\), \(s = \sin \varphi\), \(c = \cos \varphi\), and boldsymbol{hat{omega}}_times is the skew-symmetric matrix of \(\boldsymbol{\hat{\omega}}\),

\[\Phi_{11} = I_3 - \frac{\boldsymbol{\hat{\omega}}_\times}{\|\boldsymbol{\hat{\omega}}\|} s + \frac{\boldsymbol{\hat{\omega}}_\times^2}{\|\boldsymbol{\hat{\omega}}\|^2} (1 - c)\]

\[\Phi_{12} = -I_3 \Delta t_g - \frac{\boldsymbol{\hat{\omega}}_\times^2}{\|\boldsymbol{\hat{\omega}}\|^3} (\varphi - s) + \frac{\boldsymbol{\hat{\omega}}_\times}{\|\boldsymbol{\hat{\omega}}\|^2} (1 - c)\]

For \(\|\boldsymbol{\hat{\omega}}\| \Delta t_g \ll 1\), the approximation

\[\Phi_{11} \approx I_3 - \boldsymbol{\hat{\omega}}_\times \Delta t_g\]

\[\Phi_{12} \approx -I_3 \Delta t_g\]

can be used (user toggleable).

Process Noise Discretization

With gyro angle random walk density \(\sigma_v^2\) and bias random walk density \(\sigma_u^2\), the discrete process covariance over \(\Delta t_g\) is

\[\begin{split}Q = \begin{bmatrix} Q_{11} & Q_{12} \\ Q_{12} & Q_{22} \end{bmatrix}\end{split}\]

\[Q_{11} = \left( \sigma_v^2 \Delta t_g + \frac{\sigma_u^2 \Delta t_g^3}{3} \right) I_3\]

\[Q_{12} = -\frac{\sigma_u^2 \Delta t_g^2}{2} I_3\]

\[Q_{22} = \sigma_u^2 \Delta t_g I_3\]

Finally, covaraince is propagated as:

\[P \leftarrow \Phi P \Phi^T + Q\]

Star tracker measurement synthesis

At star tracker measurement events, a quaternion measurement \(\mathbf{q}_m\) of attitude is available. This is synthesized from the ground truth quaternion \(q_t\) with white noise. This is done by synthesizing a R3 vector with white noise and then converting it to a quaternion.

\[\boldsymbol{\theta}_n \sim \mathcal{N}(0, \sigma_{st}^2 I_3)\]

\[\mathbf{q}_m = \mathbf{q}_t \otimes \mathbf{q}_n \approx \mathbf{q}_t + \frac{1}{2} \Xi(\mathbf{q}_t) \boldsymbol{\theta}_n\]

Here, we used small angle approximation (as the startracker measurement error is in the order of arcseconds). Now, the estimated error quaternion for this measurement is:

\[\delta \mathbf{q}_m = \mathbf{q}_m \otimes \hat{\mathbf{q}}^{-1}\]

We then convert this to a rotation vector, explained in the first section.

\[\boldsymbol{\delta \mathbf{q}_m} \mapsto \boldsymbol{\delta\theta}_m\]

The observation model matrix is:

\[H = \begin{bmatrix} I_3 & 0 \end{bmatrix}\]

The measurement noise covariance matrix is the startracker measurement accuracy (assumed isotropic):

\[R = \sigma_{st}^2 I_3\]

We discuss the measurement update and injection in the next section, carried out in star tracker measurement events.

Measurement update and injection

Compute the innovation covariance and gain, then the correction:

\[S \leftarrow H P H^T + R\]

\[K \leftarrow P H^T S^{-1}\]

\[\boldsymbol{\delta \hat x} \leftarrow K \boldsymbol{\delta\theta}_m\]

Split \(\boldsymbol{\delta \hat x} = \begin{bmatrix} \hat{\boldsymbol{\delta\theta}} \\ \hat{\boldsymbol{\delta\beta}} \end{bmatrix}\) and inject into the global state:

\[\hat{\boldsymbol{\beta}} \leftarrow \hat{\boldsymbol{\beta}} + \hat{\delta\boldsymbol{\beta}}\]

Map the estimated error vector to a quaternion:

\[\hat{\boldsymbol{\delta \theta}} \mapsto \hat{\boldsymbol{\delta \mathbf{q}}}\]

and update the attitude multiplicatively:

\[\hat{\mathbf{q}} \leftarrow \hat{\boldsymbol{\delta \mathbf{q}}} \otimes \hat{\mathbf{q}}\]

Update the covariance (the Joseph form is used by default; a simple form is also available):

\[P \leftarrow (I_6 - K H) P (I_6 - K H)^T + K R K^T\]