# 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[12] $\hat{\bold x}_{k|k}$

P_aposteriori_k[144] ${\bold P}_{k|k}$

z[9] $\bold{z}_{k+1}$

### Output

eulerAngles[3]

Rot_matrix[9]

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

P_aposteriori[144] ${\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[0] = x_aposteriori_k[3];
wak[1] = x_aposteriori_k[4];
wak[2] = x_aposteriori_k[5];

Construct matrix O[9] 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] = 0.0F;
O[1] = -x_aposteriori_k[2];
O[2] = x_aposteriori_k[1];
O[3] = x_aposteriori_k[2];
O[4] = 0.0F;
O[5] = -x_aposteriori_k[0];
O[6] = -x_aposteriori_k[1];
O[7] = x_aposteriori_k[0];
O[8] = 0.0F;

Calculate matrix a[9] and b_a[9]. 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[0] = x_aposteriori_k[0];
x_n_b[1] = x_aposteriori_k[1];
x_n_b[2] = x_aposteriori_k[2];

Collect $\bold r_a$

b_x_aposteriori_k[0] = x_aposteriori_k[6];
b_x_aposteriori_k[1] = x_aposteriori_k[7];
b_x_aposteriori_k[2] = x_aposteriori_k[8];

Collect $\bold r_m$

z_n_b[0] = x_aposteriori_k[9];
z_n_b[1] = x_aposteriori_k[10];
z_n_b[2] = x_aposteriori_k[11];

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[6] = 0.0F;
A_lin[7] = x_aposteriori_k[8];
A_lin[8] = -x_aposteriori_k[7];
A_lin[18] = -x_aposteriori_k[8];
A_lin[19] = 0.0F;
A_lin[20] = x_aposteriori_k[6];
A_lin[30] = x_aposteriori_k[7];
A_lin[31] = -x_aposteriori_k[6];
A_lin[32] = 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[9] = 0.0F;
A_lin[10] = x_aposteriori_k[11];
A_lin[11] = -x_aposteriori_k[10];
A_lin[21] = -x_aposteriori_k[11];
A_lin[22] = 0.0F;
A_lin[23] = x_aposteriori_k[9];
A_lin[33] = x_aposteriori_k[7];
A_lin[34] = -x_aposteriori_k[9];
A_lin[35] = 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[0] = q[0];
b_q[12] = 0.0F;
b_q[24] = 0.0F;
b_q[36] = 0.0F;
b_q[48] = 0.0F;
b_q[60] = 0.0F;
b_q[72] = 0.0F;
b_q[84] = 0.0F;
b_q[96] = 0.0F;
b_q[108] = 0.0F;
b_q[120] = 0.0F;
b_q[132] = 0.0F;
b_q[1] = 0.0F;
b_q[13] = q[0];
b_q[25] = 0.0F;
b_q[37] = 0.0F;
b_q[49] = 0.0F;
b_q[61] = 0.0F;
b_q[73] = 0.0F;
b_q[85] = 0.0F;
b_q[97] = 0.0F;
b_q[109] = 0.0F;
b_q[121] = 0.0F;
b_q[133] = 0.0F;
b_q[2] = 0.0F;
b_q[14] = 0.0F;
b_q[26] = q[0];
b_q[38] = 0.0F;
b_q[50] = 0.0F;
b_q[62] = 0.0F;
b_q[74] = 0.0F;
b_q[86] = 0.0F;
b_q[98] = 0.0F;
b_q[110] = 0.0F;
b_q[122] = 0.0F;
b_q[134] = 0.0F;
b_q[3] = 0.0F;
b_q[15] = 0.0F;
b_q[27] = 0.0F;
b_q[39] = q[1];
b_q[51] = 0.0F;
b_q[63] = 0.0F;
b_q[75] = 0.0F;
b_q[87] = 0.0F;
b_q[99] = 0.0F;
b_q[111] = 0.0F;
b_q[123] = 0.0F;
b_q[135] = 0.0F;
b_q[4] = 0.0F;
b_q[16] = 0.0F;
b_q[28] = 0.0F;
b_q[40] = 0.0F;
b_q[52] = q[1];
b_q[64] = 0.0F;
b_q[76] = 0.0F;
b_q[88] = 0.0F;
b_q[100] = 0.0F;
b_q[112] = 0.0F;
b_q[124] = 0.0F;
b_q[136] = 0.0F;
b_q[5] = 0.0F;
b_q[17] = 0.0F;
b_q[29] = 0.0F;
b_q[41] = 0.0F;
b_q[53] = 0.0F;
b_q[65] = q[1];
b_q[77] = 0.0F;
b_q[89] = 0.0F;
b_q[101] = 0.0F;
b_q[113] = 0.0F;
b_q[125] = 0.0F;
b_q[137] = 0.0F;
b_q[6] = 0.0F;
b_q[18] = 0.0F;
b_q[30] = 0.0F;
b_q[42] = 0.0F;
b_q[54] = 0.0F;
b_q[66] = 0.0F;
b_q[78] = q[2];
b_q[90] = 0.0F;
b_q[102] = 0.0F;
b_q[114] = 0.0F;
b_q[126] = 0.0F;
b_q[138] = 0.0F;
b_q[7] = 0.0F;
b_q[19] = 0.0F;
b_q[31] = 0.0F;
b_q[43] = 0.0F;
b_q[55] = 0.0F;
b_q[67] = 0.0F;
b_q[79] = 0.0F;
b_q[91] = q[2];
b_q[103] = 0.0F;
b_q[115] = 0.0F;
b_q[127] = 0.0F;
b_q[139] = 0.0F;
b_q[8] = 0.0F;
b_q[20] = 0.0F;
b_q[32] = 0.0F;
b_q[44] = 0.0F;
b_q[56] = 0.0F;
b_q[68] = 0.0F;
b_q[80] = 0.0F;
b_q[92] = 0.0F;
b_q[104] = q[2];
b_q[116] = 0.0F;
b_q[128] = 0.0F;
b_q[140] = 0.0F;
b_q[9] = 0.0F;
b_q[21] = 0.0F;
b_q[33] = 0.0F;
b_q[45] = 0.0F;
b_q[57] = 0.0F;
b_q[69] = 0.0F;
b_q[81] = 0.0F;
b_q[93] = 0.0F;
b_q[105] = 0.0F;
b_q[117] = q[3];
b_q[129] = 0.0F;
b_q[141] = 0.0F;
b_q[10] = 0.0F;
b_q[22] = 0.0F;
b_q[34] = 0.0F;
b_q[46] = 0.0F;
b_q[58] = 0.0F;
b_q[70] = 0.0F;
b_q[82] = 0.0F;
b_q[94] = 0.0F;
b_q[106] = 0.0F;
b_q[118] = 0.0F;
b_q[130] = q[3];
b_q[142] = 0.0F;
b_q[11] = 0.0F;
b_q[23] = 0.0F;
b_q[35] = 0.0F;
b_q[47] = 0.0F;
b_q[59] = 0.0F;
b_q[71] = 0.0F;
b_q[83] = 0.0F;
b_q[95] = 0.0F;
b_q[107] = 0.0F;
b_q[119] = 0.0F;
b_q[131] = 0.0F;
b_q[143] = q[3];

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[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1))

Here updateVect[0] is for gyroscope, updateVect[1] is for accelerometer, updateVect[2] 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[5] < 4.0F) || (z[4] > 15.0F)) {
r[1] = 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[0] = r[0];
b_r[9] = 0.0F;
b_r[18] = 0.0F;
b_r[27] = 0.0F;
b_r[36] = 0.0F;
b_r[45] = 0.0F;
b_r[54] = 0.0F;
b_r[63] = 0.0F;
b_r[72] = 0.0F;
b_r[1] = 0.0F;
b_r[10] = r[0];
b_r[19] = 0.0F;
b_r[28] = 0.0F;
b_r[37] = 0.0F;
b_r[46] = 0.0F;
b_r[55] = 0.0F;
b_r[64] = 0.0F;
b_r[73] = 0.0F;
b_r[2] = 0.0F;
b_r[11] = 0.0F;
b_r[20] = r[0];
b_r[29] = 0.0F;
b_r[38] = 0.0F;
b_r[47] = 0.0F;
b_r[56] = 0.0F;
b_r[65] = 0.0F;
b_r[74] = 0.0F;
b_r[3] = 0.0F;
b_r[12] = 0.0F;
b_r[21] = 0.0F;
b_r[30] = r[1];
b_r[39] = 0.0F;
b_r[48] = 0.0F;
b_r[57] = 0.0F;
b_r[66] = 0.0F;
b_r[75] = 0.0F;
b_r[4] = 0.0F;
b_r[13] = 0.0F;
b_r[22] = 0.0F;
b_r[31] = 0.0F;
b_r[40] = r[1];
b_r[49] = 0.0F;
b_r[58] = 0.0F;
b_r[67] = 0.0F;
b_r[76] = 0.0F;
b_r[5] = 0.0F;
b_r[14] = 0.0F;
b_r[23] = 0.0F;
b_r[32] = 0.0F;
b_r[41] = 0.0F;
b_r[50] = r[1];
b_r[59] = 0.0F;
b_r[68] = 0.0F;
b_r[77] = 0.0F;
b_r[6] = 0.0F;
b_r[15] = 0.0F;
b_r[24] = 0.0F;
b_r[33] = 0.0F;
b_r[42] = 0.0F;
b_r[51] = 0.0F;
b_r[60] = r[2];
b_r[69] = 0.0F;
b_r[78] = 0.0F;
b_r[7] = 0.0F;
b_r[16] = 0.0F;
b_r[25] = 0.0F;
b_r[34] = 0.0F;
b_r[43] = 0.0F;
b_r[52] = 0.0F;
b_r[61] = 0.0F;
b_r[70] = r[2];
b_r[79] = 0.0F;
b_r[8] = 0.0F;
b_r[17] = 0.0F;
b_r[26] = 0.0F;
b_r[35] = 0.0F;
b_r[44] = 0.0F;
b_r[53] = 0.0F;
b_r[62] = 0.0F;
b_r[71] = 0.0F;
b_r[80] = r[2];
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}))$

Except where otherwise noted, content on this blog is licensed under a Creative Commons Attribution 4.0 International License.