# PX4 EKF Code Review

A mathematical review of PX4’s implementation of the Extended Kalman Filter(EKF) algorithm.

$\renewcommand{\bold}{\mathbf} \renewcommand{\R}{\mathbb{R}}$

# Kalman Assumption

• $\bold x_k$ state vector
• $\bold u_k$ control vector
• $\bold z_k$ measurement vector

$\bold{x}_k = \bold{A}_k \bold{x}_{k-1} + \bold{B}_k \bold{u}_k + \bold{w}_k$

• Assume $\bold{x}_k$ is a normally distributed random variable, whose mean is a linear function of $\bold{x}_{k-1}$ and $\bold{u}_k$.

$\bold{z}_k = \bold{C}_k \bold{x}_k + \bold{v}_k$

• Assume $\bold z_k$ is a normally distributed random variable, whose mean is a linear function of $\bold{x}_k$.

Multivariate Normal Distribution PDF

$p(\bold{x}) = \det(2\pi\bold\Sigma)^{-1/2} \exp\{-\dfrac{1}{2}(\bold x-\boldsymbol \mu)^T\bold\Sigma^{-1}(\bold x-\boldsymbol \mu)\}$

Transitional Probability

Define $\boldsymbol \mu_k = \bold{A}_k \bold{x}_{k-1} + \bold{B}_k \bold{u}_k$ which is the expectation of $\bold x_k$. Define $\bold{Q}_k$ to be the variance of $\bold x_k$.

$\bold x_k \sim \mathcal N(\boldsymbol \mu_k, \bold Q_k)$

In fact it is the covariance matrix (natural generalization to higher dimensions of the 1-dimensional variance)

$p(\bold{x}_k|\bold{u}_k, \bold{x}_{k-1}) = |2\pi \bold Q_k|^{-1/2} \exp[-\dfrac{1}{2}(\bold x_k-\boldsymbol \mu_k)^T \bold Q_k^{-1}(\bold x_k-\boldsymbol \mu_k)]$

Similarily, define $\boldsymbol \phi_k = \bold C_k \bold x_k$ which is the expectation of $\bold z_k$. Define $\bold R_k$ to be the variance of $\bold z_k$.

$\bold z_k \sim \mathcal N(\boldsymbol \phi_k, \bold R_k)$

$p(\bold z_k|\bold x_k) = |2\pi \bold R_k|^{-1/2} \exp[-\dfrac{1}{2}(\bold z_k-\boldsymbol\phi_k)^T \bold R_k^{-1}(\bold z_k-\boldsymbol\phi_k)]$

# Kalman Algorithm

• $\hat{\bold x}_{k|k-1}$
• priori estimate of state
• $\hat{\bold x}_{k|k}$
• posteriori estimate of state
• $\bold P_{k|k-1}$
• priori estimate of variance
• $\bold P_{k|k}$
• posteriori estimate of variance

\begin{aligned} & \textbf{Algorithm Kalman Filter} (\hat{\bold x}_{k-1|k-1}, \bold P_{k-1|k-1}, \bold u_k, \bold z_k): \\ &\qquad \hat{\bold x}_{k|k-1} = \bold A_k \hat{\bold x}_{k-1|k-1} + \bold B_k \bold u_k \\ &\qquad \bold P_{k|k-1} = \bold A_k \bold P_{k-1|k-1} \bold A_k^{T} + \bold Q_k \\ &\qquad \bold K_k = \bold P_{k|k-1} \bold C_k^T(\bold C_k \bold P_{k|k-1} \bold C_k^T + \bold R_k)^{-1}\\ &\qquad \hat{\bold x}_{k|k} = \hat{\bold x}_{k|k-1} + \bold K_k(\bold z_k - \bold C_k \hat{\bold x}_{k|k-1}) \\ &\qquad \bold P_{k|k} = (\bold I - \bold K_k \bold C_k) \bold P_{k|k-1} \\ & \textbf{return } \hat{\bold x}_{k|k}, \bold P_{k|k} \end{aligned}

1. Priori estimation of state using model mean

2. Priori estimation of variance accordingly

3. Observe measurement, calculate the measurement residual

• The model differs from measurement

• $(\bold C_k \bold P_{k|k-1} \bold C_k^T + \bold R_k)^{-1}$ is the variance of the residual

• Using the variance to calculate the Kalman gain (how much we take additional measurement into account)

• Large Kalman gain → measurement
• Small Kalman gain → model
4. Posteriori estimate of state using Kalman gain

• $(\bold z_k - \bold C_k \hat{\bold x}_{k|k-1})$ is the mean of the residual
5. Posteriori estimate of variance using Kalman gain

# Extended Kalman Algorithms

• $\bold x_k$ state vector
• $\bold u_k$ control vector
• $\bold z_k$ measurement vector

$\bold{x}_k = g(\bold{u}_k, \bold{x}_{k-1}) + \bold{w}_k \\ \bold{z}_k = h(\bold{x}_k) + \bold{v}_k \\$

$g(x)$ and $h(x)$ can be nonlinear. Expand $g(x)$ and $h(x)$ using Taylor expansion:

\begin{aligned} g(\bold u_k, \bold x_{k-1}) &\approx g(\bold u_k, \boldsymbol \mu_{k-1}) + g'(\bold u_k, \boldsymbol \mu_{k-1})(\bold x_{k-1}- \boldsymbol \mu_{k-1}) \\ &:= g(\bold u_k, \boldsymbol \mu_{k-1}) + \bold G_k(\bold x_{k-1}- \boldsymbol \mu_{k-1}) \\ h(\bold x_{k}) &\approx h(\boldsymbol \mu_{k}) + h'(\boldsymbol \mu_{k})(\bold x_{k} - \boldsymbol \mu_{k}) \\ &:= h(\boldsymbol \mu_{k}) + \bold H_k(\bold x_{k} - \boldsymbol \mu_{k}) \\ \end{aligned}

$\bold G$ and $\bold H$ correspond to $\bold A$ and $\bold C$ in Kalman Filter.

\begin{aligned} & \textbf{Algorithm Extended Kalman Filter} (\hat{\bold x}_{k-1|k-1}, \bold P_{k-1|k-1}, \bold u_k, \bold z_k): \\ &\qquad \hat{\bold x}_{k|k-1} = g(\bold u_k, \hat{\bold x}_{k-1|k-1}) \\ &\qquad \bold P_{k|k-1} = \bold G_k \bold P_{k-1|k-1} \bold G_k^{T} + \bold Q_k \\ &\qquad \bold K_k = \bold P_{k|k-1} \bold H_k^T(\bold H_k \bold P_{k|k-1} \bold H_k^T + \bold R_k)^{-1}\\ &\qquad \hat{\bold x}_{k|k} = \hat{\bold x}_{k|k-1} + \bold K_k(\bold z_k - h(\hat{\bold x}_{k|k-1})) \\ &\qquad \bold P_{k|k} = (\bold I - \bold K_k \bold H_k) \bold P_{k|k-1} \\ & \textbf{return } \hat{\bold x}_{k|k}, \bold P_{k|k} \end{aligned}

# Example

## State Vector

$\bold x = \begin{bmatrix} \boldsymbol \omega \\ \dot{\boldsymbol \omega} \\ \bold r_a \\ \bold r_m \end{bmatrix}$

• $\bold x \in \R^{12\times1}$
• $\boldsymbol\omega = [\omega_x, \omega_y, \omega_z]^T$, real angular velocity
• $\dot{\boldsymbol\omega} = [\dot\omega_x, \dot\omega_y,\dot \omega_z]^T$, real angular acceleration
• $\bold r_a = [r_{ax}, r_{ay}, r_{az}]^T$, real acceleration
• $\bold r_m = [r_{mx}, r_{my}, r_{mz}]^T$, real magnetic field

## Measurement vector

$\bold z = \begin{bmatrix} \bar{\boldsymbol\omega} \\ \bar{\bold r}_a \\ \bar{\bold r}_m \end{bmatrix}$

• $\bold z \in \R^{9 \times 1}$
• $\bar{\boldsymbol\omega}$, measured angular velocity from gyroscope
• $\bar{\bold r}_a$, measured acceleration from accelerometer
• $\bar{\bold r}_m$, measured magnetic field from magnetometer

## Prediction Model

$\bold x_{k+1} = g(\bold x_k)+ \bold w_k \approx \begin{bmatrix} \boldsymbol \omega_k + \dot{\boldsymbol \omega}_k \Delta t \\ \dot{\boldsymbol \omega}_k \\ \bold r_{a,k} + \tilde{\boldsymbol\omega}_k \bold r_{a,k} \Delta t \\ \bold r_{m,k} + \tilde{\boldsymbol\omega}_k \bold r_{m,k} \Delta t \\ \end{bmatrix} + \begin{bmatrix} \bold w_{\omega_k} \\ \bold w_{\dot\omega_k} \\ \bold w_{r_{a, k}} \\ \bold w_{r_{m, k}} \\ \end{bmatrix}$

$\bold w_k$ is Guassian noise. $\tilde{\boldsymbol\omega}_k$ is the direction cosine matrix(DCM)

$\tilde{\boldsymbol\omega}_k = \begin{bmatrix} 0 & \omega_z & -\omega_y\\ -\omega_z & 0 & \omega_x \\ \omega_y & -\omega_x & 0 \end{bmatrix} \\ \tilde{\boldsymbol\omega}_k \bold r_{a,k} = \begin{bmatrix} r_{ay,k}\omega_z - r_{az,k}\omega_y\\ -r_{ax,k}\omega_z + r_{az,k}\omega_x \\ r_{ax,k}\omega_y - r_{ay,k} \omega_x \end{bmatrix}\\ \dfrac{\partial \tilde{\boldsymbol\omega}_k \bold r_{a,k}}{\partial \boldsymbol\omega_k} = \begin{bmatrix} 0 & -r_{az,k} & r_{ay,k}\\ r_{az,k} & 0 & -r_{ax,k} \\ -r_{ay,k} & r_{ax,k} & 0 \end{bmatrix} := -\tilde{\bold r}_{a,k}$

• Angular velocity changes by angular acceleration times infinitesimal time
• Ignore change of angular acceleration
• Acceleration and Magnetometor are subject to change of attitude, which is DCM

Therefore the model matrix $\bold G$ can be constructed as

$\bold G_{k} = \dfrac{\partial g(\bold x_k)}{\partial \bold x_k} = \begin{bmatrix} \bold I & \bold I\Delta t & 0 &0 \\ 0 & \bold I & 0 & 0 \\ -\tilde{\bold r}_{a,k}\Delta t & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t & 0 \\ -\tilde{\bold r}_{m,k}\Delta t & 0 & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t \\ \end{bmatrix}$

$\bold G_{k} \in \R^{12 \times 12}$.

The relationship between measurement and state can be characterized as

$\bold z_k = \begin{bmatrix} \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold I & 0 \\ 0 & 0 & 0 & \bold I\\ \end{bmatrix} \bold x_k + \begin{bmatrix} \bold v_{\bar \omega} \\ \bold v_{\bar r_a} \\ \bold v_{\bar r_m} \\ \end{bmatrix}$

$\bold v_k$ is measurement noise. $\bold H_k \in \R^{9 \times 12}$ .

## Procedure

### Input

x_aposteriori_k $\hat{\bold x}_{k|k}$

P_aposteriori_k ${\bold P}_{k|k}$

z $\bold{z}_{k+1}$

### Output

eulerAngles

Rot_matrix

x_aposteriori $\hat{\bold x}_{k+1|k+1}$

P_aposteriori ${\bold P}_{k+1|k+1}$

### Priori Estimate of State

$\hat{\bold x}_{k+1|k} = g(\bold u_{k+1}, \hat{\bold x}_{k|k})$

Get the angular acceleration from last moment

wak = x_aposteriori_k;
wak = x_aposteriori_k;
wak = x_aposteriori_k;

Construct matrix O which is $\tilde {\boldsymbol \omega}_k^T$

$\bold O = \begin{bmatrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \\$

O = 0.0F;
O = -x_aposteriori_k;
O = x_aposteriori_k;
O = x_aposteriori_k;
O = 0.0F;
O = -x_aposteriori_k;
O = -x_aposteriori_k;
O = x_aposteriori_k;
O = 0.0F;

Calculate matrix a and b_a. dv0 is the $3 \times 3$ unit matrix

$\bold a = \bold{ba} = \bold I + \bold O \Delta t= (\bold I + \tilde {\boldsymbol \omega}_k \Delta t)^T$

eye(dv0);
for (i = 0; i < 9; i++) {
a[i] = (real32_T)dv0[i] + O[i] * dt;
}
for (i = 0; i < 9; i++) {
b_a[i] = (real32_T)dv0[i] + O[i] * dt;
}

Remember that

$\bold x = \begin{bmatrix} \boldsymbol \omega \\ \dot{\boldsymbol \omega} \\ \bold r_a \\ \bold r_m \end{bmatrix}$

Collect $\boldsymbol\omega_k$

x_n_b = x_aposteriori_k;
x_n_b = x_aposteriori_k;
x_n_b = x_aposteriori_k;

Collect $\bold r_a$

b_x_aposteriori_k = x_aposteriori_k;
b_x_aposteriori_k = x_aposteriori_k;
b_x_aposteriori_k = x_aposteriori_k;

Collect $\bold r_m$

z_n_b = x_aposteriori_k;
z_n_b = x_aposteriori_k;
z_n_b = x_aposteriori_k;

Calculate

$\bold{ca} = \bold O^T \bold r_a = (\bold I + \tilde {\boldsymbol \omega}_k \Delta t) \bold r_a \\ \bold{da} = \bold O^T \bold r_m = (\bold I + \tilde {\boldsymbol \omega}_k \Delta t) \bold r_m$

for (i = 0; i < 3; i++) {
c_a[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; // r_a
}
d_a[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; // r_m
}
x_apriori[i] = x_n_b[i] + dt * wak[i]; // add angular velocity times delta t
}

The prior estimation is calculated using linear approximation.

Remember that

$\bold G_{k+1} = \begin{bmatrix} \bold I & \bold I\Delta t & 0 &0 \\ 0 & \bold I & 0 & 0 \\ -\tilde{\bold r}_{a,k}\Delta t & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t & 0 \\ -\tilde{\bold r}_{m,k}\Delta t & 0 & 0 & \bold I+ \tilde{\boldsymbol\omega}_k \Delta t \\ \end{bmatrix}$

$\hat{\bold x}_{k+1|k} = g(\bold u_{k+1}, \hat{\bold x}_{k|k}) \approx \bold G_{k+1} \hat{\bold x}_{k|k}$

• Suppose there is no control vector, i.e. $\bold u_{k+1} =0$
• Partition $\hat{\bold x}_{k+1|k}$ into four vertical parts, then
• $\hat{\bold x}_{k+1|k, 1} = \boldsymbol \omega + \dot{\boldsymbol \omega} \Delta t$
• This is done in the above code
• $\hat{\bold x}_{k+1|k, 2} = \dot{\boldsymbol \omega}$
• $\hat{\bold x}_{k+1|k, 3} = \bold{ca}$
• Notice that the part $-\tilde{\bold r}_{a,k}\Delta t$ is ignored
• $\hat{\bold x}_{k+1|k, 4} = \bold{da}$

• Notice that the part $-\tilde{\bold r}_{a,m}\Delta t$ is ignored
for (i = 0; i < 3; i++) {
x_apriori[i + 3] = wak[i];
}
for (i = 0; i < 3; i++) {
x_apriori[i + 6] = c_a[i];
}
for (i = 0; i < 3; i++) {
x_apriori[i + 9] = d_a[i];
}

Priori Estimate of State Finished

### Priori Estimate of Variance

$\bold P_{k+1|k} = \bold G_{k+1} \bold P_{k|k} \bold G_{k+1}^{T} + \bold Q_{k+1}$

Calculate $\bold G_k$. First Calculate

$\bold {Alin} = \begin{bmatrix} 0 & 0 & -\tilde{\bold r}_{a,k}^T & -\tilde{\bold r}_{a,m}^T \\ \bold I & 0 & 0 & 0 \\ 0 & 0 & \tilde{\boldsymbol\omega}_k^T & 0 \\ 0 & 0 & 0 & \tilde{\boldsymbol\omega}_k^T \\ \end{bmatrix} = \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & \tilde{\bold r}_{a,m} \\ \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold O & 0 \\ 0 & 0 & 0 & \bold O \\ \end{bmatrix}$

$\bold{iv0} = \begin{bmatrix} 0 \\ \bold I \\ 0 \\ 0 \\ \end{bmatrix} \qquad \bold {Alin} \leftarrow \begin{bmatrix} 0 & 0 & ? & ? \\ \bold I & 0 & ? & ? \\ 0 & 0 & ? & ? \\ 0 & 0 & ? & ? \\ \end{bmatrix}$

for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
}

for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * i) + 3] = 0.0F;
}
}

$\bold{Alin} \leftarrow \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & ? \\ \bold I & 0 & 0 & ? \\ 0 & 0 & ? & ? \\ 0 & 0 & ? & ? \\ \end{bmatrix}$

A_lin = 0.0F;
A_lin = x_aposteriori_k;
A_lin = -x_aposteriori_k;
A_lin = -x_aposteriori_k;
A_lin = 0.0F;
A_lin = x_aposteriori_k;
A_lin = x_aposteriori_k;
A_lin = -x_aposteriori_k;
A_lin = 0.0F;

for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
}
}

$\bold{Alin} \leftarrow \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & ? \\ \bold I & 0 & 0 & ? \\ 0 & 0 & \bold O & ? \\ 0 & 0 & 0 & ? \\ \end{bmatrix}$

for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
}
}

for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
}
}

$\bold{Alin} \leftarrow \begin{bmatrix} 0 & 0 & \tilde{\bold r}_{a,k} & \tilde{\bold r}_{a,m} \\ \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold O & 0 \\ 0 & 0 & 0 & \bold O \\ \end{bmatrix}$

A_lin = 0.0F;
A_lin = x_aposteriori_k;
A_lin = -x_aposteriori_k;
A_lin = -x_aposteriori_k;
A_lin = 0.0F;
A_lin = x_aposteriori_k;
A_lin = x_aposteriori_k;
A_lin = -x_aposteriori_k;
A_lin = 0.0F;

for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
}
}

for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
}
}

for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
}
}

$\bold{dv1} = \begin{bmatrix} \bold I & 0 & 0 & 0 \\ 0 & \bold I & 0 & 0 \\ 0 & 0 & \bold I & 0 \\ 0 & 0 & 0 & \bold I \\ \end{bmatrix} \qquad \bold{bAlin} := \bold G_{k+1}^T = \bold{dv1} + \bold{Alin} \Delta t$

for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *dt;
}
}

G Calculation Finished

Setup noise coefficient matrix $\bold Q$

• We view $\bold Q_{k+1} = \bold G_{k+1} \bold Q \bold G_{k+1}^T$

$\bold Q = \begin{bmatrix} \bold q_1 & 0 & 0 & 0 \\ 0 & \bold q_2 & 0 & 0 \\ 0 & 0 & \bold q_3 & 0 \\ 0 & 0 & 0 & \bold q_4 \\ \end{bmatrix} \qquad \bold q_i = \begin{bmatrix} q_i & 0 & 0 \\ 0 & q_i & 0 \\ 0 & 0 & q_i \\ \end{bmatrix}$

b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = 0.0F;
b_q = q;

Utilize input $\bold P_{k|k}$ to do the next estimate

$\bold{Alin}_{j,i} \leftarrow \bold{bAlin}_{:,i} \cdot \bold{P}_{k|k,j,:} = \bold{P}_{k|k,j,:} \cdot \bold{bAlin}_{:,i} \\ \text{or equivalently} \\ \bold{Alin} \leftarrow \bold P_{k|k}\bold G_{k+1}^T \\ \bold{cAlin} \leftarrow \bold Q \bold G_{k+1}^T \\ \bold{dAlin} \leftarrow \bold G_{k+1} \bold{Alin} = \bold G_{k+1} \bold P_{k|k}\bold G_{k+1}^T \\ \bold{eAlin} \leftarrow \bold G_{k+1} \bold{cAlin} = \bold G_{k+1} \bold Q \bold G_{k+1}^T \\$

for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0];
}

c_A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
}
}

for (i0 = 0; i0 < 12; i0++) {
d_A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}

e_A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}
}
}

Finally

$\bold P_{k+1|k} = \bold{dAlin} + \bold{eAlin} = \bold G_{k+1} \bold P_{k|k}\bold G_{k+1}^T + \bold G_{k+1} \bold Q \bold G_{k+1}^T$

for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
}
}

Priori Estimate of Variance Finished

### Calculate Kalman Gain

$\bold K_{k+1} = \bold P_{k+1|k} \bold H_{k+1}^T(\bold H_{k+1} \bold P_{k+1|k} \bold H_{k+1}^T + \bold R_{k+1})^{-1}$

First make sure all sensor values have been updated

if ((updateVect == 1) && (updateVect == 1) && (updateVect == 1))

Here updateVect is for gyroscope, updateVect is for accelerometer, updateVect is for magnetometer.

Remember that $\bold H_{k+1}$ is considered as a constant $\in \R^{9 \times 12}$

$\bold {iv1} = \bold H = \begin{bmatrix} \bold I & 0 & 0 & 0 \\ 0 & 0 & \bold I & 0 \\ 0 & 0 & 0 & \bold I\\ \end{bmatrix} \\ \bold{bP}_{k+1|k} = \bold H \bold{P}_{k+1|k}$

if ((z < 4.0F) || (z > 15.0F)) {
r = 10000.0F;
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 9; i0++) {
b_P_apriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + 12 * i0];
}
}
}

$\bold {iv2} = \bold H^T = \begin{bmatrix} \bold I & 0 & 0 \\ 0 & 0 & 0 \\ 0 & \bold I & 0 \\ 0 & 0 & \bold I\\ \end{bmatrix} \\ \bold{Temp} \leftarrow \bold{P}_{k+1|k} \bold{iv2} = \bold{P}_{k+1|k}\bold H^T \\ \bold {fv0} \leftarrow \bold{iv1} \bold{Temp} = \bold H \bold{P}_{k+1|k}\bold H^T$

for (i = 0; i < 9; i++) {
for (i0 = 0; i0 < 12; i0++) {
K_k[i + 9 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
}
}

for (i0 = 0; i0 < 9; i0++) {
fv0[i + 9 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
}
}
}

Setup noise coefficient matrix $\bold R$

• We view $\bold R_{k+1} = \bold R$.

$\bold R = \begin{bmatrix} \bold r_1 & 0 & 0 \\ 0 & \bold r_2 & 0 \\ 0 & 0 & \bold r_3 \end{bmatrix} \qquad \bold r_i = \begin{bmatrix} r_i & 0 & 0 \\ 0 & r_i & 0 \\ 0 & 0 & r_i \\ \end{bmatrix} \qquad \bold{fv1} = \bold{fv0} + \bold R = \bold H \bold{P}_{k+1|k}\bold H^T + \bold R$

b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = 0.0F;
b_r = r;
for (i = 0; i < 9; i++) {
for (i0 = 0; i0 < 9; i0++) {
fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
}
}

Now perform matrix division to obtain Kalman Gain.

$\bold K_{k+1}(\bold H_{k+1} \bold P_{k+1|k} \bold H_{k+1}^T + \bold R_{k+1}) = \bold P_{k+1|k} \bold H_{k+1}^T \\ \bold P_{k+1|k} \bold H_{k+1}^T =\bold{bP}_{k+1|k}^T \in \R^{12 \times 9}, \quad \bold K_{k+1} \in \R^{12 \times 9}, \quad (\bold H_{k+1} \bold P_{k+1|k} \bold H_{k+1}^T + \bold R_{k+1})=\bold{fv1} \in \R^{9 \times 9}$

mrdivide(b_P_apriori, fv1, K_k);

Kalman Gain Finished

### Posteriori Estimate of State

$\hat{\bold x}_{k+1|k} = \hat{\bold x}_{k+1|k} + \bold K_{k+1}(\bold z_{k+1} - h(\hat{\bold x}_{k+1|k}))$