# Process: F, Q The process of the kalman filter is described by $\mathbf F$ (the state transition function) and $\mathbf Q$ (the process covariance). First, let's address $\mathbf F$. There are a few things we want $\mathbf F$ to do. 1. Position Update If $dt$ is the time between measurements, then we want it to update the postion $\mathbf p = \left[p^\text{N}_k, p^\text{E}_k, p^\text{D}_k\right]^T$ in a way that satisfies $$ \mathbf p_{k+1} = \mathbf p_k + (\mathbf v_k)dt, $$ where $\mathbf v = \left[v^\text{N}_k, v^\text{E}_k, v^\text{D}_k\right]^T$, and $dt$ is the time step between measurements. This expands to $$ \begin{bmatrix}p^\text N_{k+1}\\p^\text E_{k+1}\\p^\text D_{k+1}\end{bmatrix} = \begin{bmatrix}p^\text N_{k}\\p^\text E_{k}\\p^\text D_{k}\end{bmatrix} + \begin{bmatrix}v^\text N_{k}\\v^\text E_{k}\\v^\text D_{k}\end{bmatrix}dt. $$ Therefore, the top three rows of our matrix will be $$ \begin{bmatrix} 1&0&0&dt&0&0&0&0&0&0&0&0&0&0&0&0\\ 0&1&0&0&dt&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&1&0&0&dt&0&0&0&0&0&0&0&0&0&0 \end{bmatrix}. $$ 2. Velocity Update We want it to update the velocity in way that satisfies $$ \mathbf v_{k+1} = \mathbf v_k + (\mathbf a_k - g)dt, $$ where the $-g$ term corrects for gravity. This expands to $$ \begin{align*} \begin{bmatrix}v^\text N_{k+1}\\v^\text E_{k+1}\\v^\text D_{k+1}\end{bmatrix} &= \begin{bmatrix}v^\text N_{k}\\v^\text E_{k}\\v^\text D_{k}\end{bmatrix} + \left(\begin{bmatrix}a^\text N_{k}\\a^\text E_{k}\\a^\text D_{k}\end{bmatrix} - \begin{bmatrix}0\\0\\1\end{bmatrix}\right)dt, \\ &= \begin{bmatrix}v^\text N_{k}\\v^\text E_{k}\\v^\text D_{k}\end{bmatrix} + \begin{bmatrix}a^\text N_{k}\\a^\text E_{k}\\a^\text D_{k}-1\end{bmatrix}dt, \end{align*} $$ Since the resulting $-dt$ term at the bottom of the vector is non linearly dependent on the components of $\mathbf x$, we can model gravity's input to the system by letting $\mathbf{Bu}$ subtract $dt$ from the $v^\text D$ component of $\mathbf x$ in the state update equation , and the next three rows of our matrix will be $$ \begin{bmatrix} 0&0&0&1&0&0&dt&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&1&0&0&dt&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&1&0&0&dt&0&0&0&0&0&0&0 \end{bmatrix}. $$ 3. Acceleration Update To keep things simple, we won't predict change to the acceleration or angular velocity and will use $$ \mathbf a_{k+1} = \mathbf a_k. $$ Therefore, next three rows of our matrix will be $$ \begin{bmatrix} 0&0&0&0&0&0&1&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&1&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&1&0&0&0&0&0&0&0 \end{bmatrix}. $$ 4. Orientation Update We want it to update the orientation in a way that satisfies the quaternion update function $$ \mathbf q_{k+1} = \mathbf q_k+\frac12dt\cdot\mathbf q_k\otimes\begin{bmatrix}0 \\ \omega x_k \\ \omega y_k \\ \omega z_k\end{bmatrix}. $$ satisfy our rotation update equation.Let's start by expanding our quaternion multiplication term. We know that the product of two quaternions $$ \mathbf q_1 = (w_1 + x_1i + y_1j + z_1k) $$ and $$ \mathbf q_2 = (w_2 + x_2i + y_2j + z_2k) $$ is calculated using the formula: $$ \begin{align*} \mathbf q=\mathbf q_1 \otimes \mathbf q_2=&\ \ \ \left(w_1w_2 - x_1x_2 - y_1y_2 - z_1z_2\right) \\ &+ \left(w_1x_2 + x_1w_2 + y_1z_2 - z_1y_2\right)i \\ &+ \left(w_1y_2 - x_1z_2 + y_1w_2 + z_1x_2\right)j \\ &+ \left(w_1z_2 + x_1y_2 - y_1x_2 + z_1w_2\right)k. \end{align*} $$ Substituting $$ \begin{align*} \begin{bmatrix}w_1\\x_1\\y_1\\z_1\end{bmatrix} &= \begin{bmatrix}q^0_k\\q^1_k\\q^2_k\\q^3_k\end{bmatrix},\\ \begin{bmatrix}w_2\\x_2\\y_2\\z_2\end{bmatrix} &= \begin{bmatrix}0 \\ \omega^N_k \\ \omega^E_k \\ \omega^D_k\end{bmatrix}, \end{align*} $$ gives $$ \begin{align*} \mathbf q_k\otimes\begin{bmatrix}0 \\ \omega^N_k \\ \omega^E_k \\ \omega^D_k\end{bmatrix} &= \begin{bmatrix}q^0_k\\q^1_k\\q^2_k\\q^3_k\end{bmatrix}\begin{bmatrix}0 \\ \omega^N_k \\ \omega^E_k \\ \omega^D_k\end{bmatrix} \\ &=\ \ \ \left(q^0_k0 - q^1_k\omega^N_k - q^2_k\omega^E_k - q^3_k\omega^D_k\right) \\ &\ \ \ \ + \left(q^0_k\omega^N_k + q^1_k0 + q^2_k\omega^D_k - q^3_k\omega^E_k\right)i \\ &\ \ \ \ + \left(q^0_k\omega^E_k - q^1_k\omega^D_k + q^2_k0 + q^3_k\omega^N_k\right)j \\ &\ \ \ \ + \left(q^0_k\omega^D_k + q^1_k\omega^E_k - q^2_k\omega^N_k + q^3_k0\right)k. \end{align*} $$ Writing this result in vector form, we have $$ \begin{bmatrix} (q^0_k)(0) &- (q^1_k)(\omega^N_k) &- (q^2_k)(\omega^E_k) &- (q^3_k)(\omega^D_k) \\ (q^0_k)(\omega^N_k) &+ (q^1_k)(0) &+ (q^2_k)(\omega^D_k) &- (q^3_k)(\omega^E_k) \\ (q^0_k)(\omega^E_k) &- (q^1_k)(\omega^D_k) &+ (q^2_k)(0) &+ (q^3_k)(\omega^N_k) \\ (q^0_k)(\omega^D_k) &+ (q^1_k)(\omega^E_k) &- (q^2_k)(\omega^N_k) &+ (q^3_k)(0) \\ \end{bmatrix}. $$ We see that each component is in the form $[a(q0_k)+b(q1_k)+c(q2_k)+d(q3_k)]$, for some constants $a$, $b$, $c$, and $d$. This is looking quite close to the form we would like for our state transition matrix! We can substite $$ \mathbf q_k\otimes\begin{bmatrix}0 \\ \omega_x \\ \omega_y \\ \omega_z\end{bmatrix} = \begin{bmatrix} (q^0_k)(0) &- (q^1_k)(\omega^N_k) &- (q^2_k)(\omega^E_k) &- (q^3_k)(\omega^D_k) \\ (q^0_k)(\omega^N_k) &+ (q^1_k)(0) &+ (q^2_k)(\omega^D_k) &- (q^3_k)(\omega^E_k) \\ (q^0_k)(\omega^E_k) &- (q^1_k)(\omega^D_k) &+ (q^2_k)(0) &+ (q^3_k)(\omega^N_k) \\ (q^0_k)(\omega^D_k) &+ (q^1_k)(\omega^E_k) &- (q^2_k)(\omega^N_k) &+ (q^3_k)(0) \\ \end{bmatrix} $$ into our rotation update equation to get $$ \mathbf q_{k+1} = \mathbf q_k+\frac12dt\cdot \begin{bmatrix} (q^0_k)(0) &- (q^1_k)(\omega^N_k) &- (q^2_k)(\omega^E_k) &- (q^3_k)(\omega^D_k) \\ (q^0_k)(\omega^N_k) &+ (q^1_k)(0) &+ (q^2_k)(\omega^D_k) &- (q^3_k)(\omega^E_k) \\ (q^0_k)(\omega^E_k) &- (q^1_k)(\omega^D_k) &+ (q^2_k)(0) &+ (q^3_k)(\omega^N_k) \\ (q^0_k)(\omega^D_k) &+ (q^1_k)(\omega^E_k) &- (q^2_k)(\omega^N_k) &+ (q^3_k)(0) \\ \end{bmatrix}. $$ Writing the whole right side as one vector gives $$ \mathbf q_{k+1} = \begin{bmatrix} q^0_k + (dt/2)((q^0_k)(0) - (q^1_k)(\omega^N_k) - (q^2_k)(\omega^E_k) - (q^3_k)(\omega^D_k)) \\ q^1_k + (dt/2)((q^0_k)(\omega^N_k) + (q^1_k)(0) + (q^2_k)(\omega^D_k) - (q^3_k)(\omega^E_k)) \\ q^2_k + (dt/2)((q^0_k)(\omega^E_k) - (q^1_k)(\omega^D_k) + (q^2_k)(0) + (q^3_k)(\omega^N_k)) \\ q^3_k + (dt/2)((q^0_k)(\omega^D_k) + (q^1_k)(\omega^E_k) - (q^2_k)(\omega^N_k) + (q^3_k)(0)) \\ \end{bmatrix}. $$ Therefore, next four rows of our matrix will be $$ \begin{bmatrix} 0&0&0&0&0&0&0&0&0&1&-(dt\cdot\omega^N_k)/2&-(dt\cdot\omega^E_k)/2&-(dt\cdot\omega^D_k)/2&0&0&0\\ 0&0&0&0&0&0&0&0&0&(dt\cdot\omega^N_k)/2&1&(dt\cdot\omega^D_k)/2&-(dt\cdot\omega^E_k)/2&0&0&0\\ 0&0&0&0&0&0&0&0&0&(dt\cdot\omega^E_k)/2&-(dt\cdot\omega^D_k)/2&1&(dt\cdot\omega^N_k)/2&0&0&0\\ 0&0&0&0&0&0&0&0&0&(dt\cdot\omega^D_k)/2&(dt\cdot\omega^E_k)/2&-(dt\cdot\omega^N_k)/2&1&0&0&0 \end{bmatrix}. $$ 5. Angular Velocity Update $$ \boldsymbol\omega_{k+1} = \boldsymbol\omega_k. $$ Therefore, last three rows of our matrix will be $$ \begin{bmatrix} 0&0&0&0&0&0&0&0&0&0&0&0&0&1&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0&0&0&1&0\\ 0&0&0&0&0&0&0&0&0&0&0&0&0&0&0&1 \end{bmatrix}. $$ We now have $$ \mathbf F = \begin{bmatrix} 1&0&0&dt&0&0&0&0&0&0&0&0&0&0&0&0\\ 0&1&0&0&dt&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&1&0&0&dt&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&1&0&0&dt&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&1&0&0&dt&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&1&0&0&dt&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&1&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&1&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&1&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&1&-(dt\cdot\omega^N_k)/2&-(dt\cdot\omega^E_k)/2&-(dt\cdot\omega^D_k)/2&0&0&0\\ 0&0&0&0&0&0&0&0&0&(dt\cdot\omega^N_k)/2&1&(dt\cdot\omega^D_k)/2&-(dt\cdot\omega^E_k)/2&0&0&0\\ 0&0&0&0&0&0&0&0&0&(dt\cdot\omega^E_k)/2&-(dt\cdot\omega^D_k)/2&1&(dt\cdot\omega^N_k)/2&0&0&0\\ 0&0&0&0&0&0&0&0&0&(dt\cdot\omega^D_k)/2&(dt\cdot\omega^E_k)/2&-(dt\cdot\omega^N_k)/2&1&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0&0&1&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0&0&0&1&0\\ 0&0&0&0&0&0&0&0&0&0&0&0&0&0&0&1\\ \end{bmatrix}. $$ *Notes:* - *We were able to write lines 10-13 of this matrix using our observation that each component of $\mathbf q_{k+1}$ was of the form $[a(q^0_k)+b(q^1_k)+c(q^2_k)+d(q^3_k)]$, but we could have also found the form of the $\zeta^\text{th}$ component to be $[q^\zeta_k + a(\omega^N_k)+b(\omega^E_k)+c(\omega^D_k)]$. This second form will let us write a second, equavalent matrix to help double check we have placed our values correctly.* - *This state transition matrix assumes acceleration at time $t_{k+1}$ will approximately equal acceleration at time $t_k$, which is not necessarily true during jerky events such as heel strike.*