 |
Master Thesis Code
by Simon Moser
|
Last updated: 7. May 2024
As we have seen in Part 8: 6D EKF-RTS the state prediction for both the acceleration and angular velocity are not accurate. They're both assumed to be constant. Since gait processes are difficult to predict, a different approach is taken. Both the measurements of the angular velocity and the acceleration are moved to the control input. The approach of moving measurements to the control input is common and used in a variety of literature (Goslinski et al., 2015; Kok et al., 2017; Sabatini, 2011).
Clearly, this introduces also measurement noise directly on the predictions, which is not ideal. However, considering as an example the walking task from the last part, the changes in the acceleration measurements from sample to sample were up to 150 m/s² and 70 m/s² for stamping and walking, respectively. The sample to sample differences in the angular velocity were up to 900 °/s and 250 °/s for stamping and walking, respectively.
Compared to the standard deviation of the measurement noise which is around 0.045 m/s² and 0.071 °/s for the acceleration and angular velocity, respectively, the changes in the measurements are much larger. Therefore, the improvements in the prediction by using the measurements are expected to outweigh the introduction of noise on the predictions leading to much more accurate predictions.
Therefore in the following derivation, the general formulation for an EKF and RTS smoother containing control inputs is presented. Then, the specific formulation for the 6D EKF-RTS is derived and implemented in an example.
In the following table the differences between the standard EKF and the EKF with control inputs (CI) are shown. As you can see, the only difference is the addition of the control input to the state prediction. The table is an extension to the table in derivation4Ekf6D.
| Equation | EKF without CI | EKF with CI | |
|---|---|---|---|
| Prediction | State extrapolation | \( \hat{x}_{f,k+1}^- = f\left(\hat{x}_{f,k}^+\right) \) | \( \hat{x}_{f,k+1}^- = f\left(\hat{x}_{f,k}^+\right) \color{red}{+ B_k u_k} \) |
| Covariance extrapolation | \( P_{f,k+1}^- = F_k P_{f,k}^+ \left(F_k\right)^T + Q \) | \( P_{f,k+1}^- = F_k P_{f,k}^+ \left(F_k\right)^T + Q \) | |
| Update | Kalman gain | \begin{align*} K_{f,k} &= P_{f,k}^- \left(H_k\right)^T \\ &\times \left(H_k P_{f,k}^- \left(H_k\right)^T + R\right)^{-1} \end{align*} | \begin{align*} K_{f,k} &= P_{f,k}^- \left(H_k\right)^T \\ &\times \left(H_k P_{f,k}^- \left(H_k\right)^T + R\right)^{-1} \end{align*} |
| State update | \( \hat{x}_{f,k}^+ = \hat{x}_{f,k}^- + K_{f,k} \left(z_k - h\left(\hat{x}_{f,k}^-\right)\right) \) | \( \hat{x}_{f,k}^+ = \hat{x}_{f,k}^- + K_{f,k} \left(z_k - h\left(\hat{x}_{f,k}^-\right)\right) \) | |
| Covariance update | \begin{align*} P_{f,k}^+ &= \left(I - K_{f,k} H_k\right) P_{f,k}^- \\ &\times \left(I - K_{f,k} H_k\right)^T + K_{f,k} R K_{f,k}^T \end{align*} | \begin{align*} P_{f,k}^+ &= \left(I - K_{f,k} H_k\right) P_{f,k}^- \\ &\times \left(I - K_{f,k} H_k\right)^T + K_{f,k} R K_{f,k}^T \end{align*} |
The formulation for the RTS smoother stays the same and is given by
\begin{align*} \mathcal{I}_{f,k+1}^- &= \left(P_{f,k+1}^-\right)^{-1} \\[1em] K_{b,k} &= P_{f,k}^+ F_k^T \mathcal{I}_{f,k+1}^- \\[1em] \hat{x}_{b,k}^+ &= \hat{x}_{f,k}^+ + K_{b,k}(\hat{x}_{b,k+1}^+ - \hat{x}_{f,k+1}^-) \\[1em] P_{b,k}^+ &= P_{f,k}^+ - K_{b,k}(P_{f,k+1}^- - P_{b,k+1}^+) K_{b,k}^T. \end{align*}
As previously mentioned, we have two false assumptions in our previous state extrapolation model. The first one is that the acceleration is constant and the second one is that the angular velocity is constant. As shown in the last part, these assumptions introduce lags in the state estimation. Therefore we want to:
Therefore, we can remove the angular velocity and the acceleration from the state vector.
The state vector is then defined as
\[ \mathbf{x} = \begin{bmatrix} \mathbf{q} & \prescript{g}{}{\mathbf{v}} & \prescript{g}{}{\mathbf{p}} & \mathbf{b}_a & \mathbf{b}_\omega & \mathbf{s}_a \end{bmatrix}^T \]
where
leaving us with a state vector of \( n = 19 \) elements.
As mentioned above, we cannot strictly separate the control input from the state. Rather, we still do the same exact state extrapolation in terms of the formulation of the function, but we exchange the acceleration and angular velocity variables with measurements.
To make it even more clear, let's consider the prediction of the orientation. Before we can calculate the difference quaternion based on the current angular velocity, we need to correct the angular velocity measurement by its bias. We call this preprocessing. The same is done for the acceleration measurement. In order to distinguish between the variables, we need to introduce a new notation to distinguish between them. We denote the preprocessed variables with a tilde, e.g. \( \prescript{g}{}{\tilde{\mathbf{a}}} \). Estimated variables from the filter are denoted with a hat, e.g. \( \prescript{g}{}{\hat{\mathbf{v}}} \).
Also, because we cannot separate the control input, we'll stick with the state transition beeing a single function \( \mathbf{f} \) which takes the state vector and the control input as arguments. The state transition function is then defined as
\begin{align*} \mathbf{\hat{x}}_{f,k+1}^- &= \mathbf{f}\left(\mathbf{\hat{x}}_{f,k}^+, \prescript{s}{}{\tilde{\boldsymbol{\omega}}_k}, \prescript{g}{}{\mathbf{\tilde{a}}_k}\right) \\[1em] &= \begin{bmatrix} \mathbf{f}_1(\mathbf{\hat{q}}_{f,k}^+, \prescript{s}{}{\boldsymbol{\tilde{\omega}}_k}) \hfill \\ \mathbf{f}_2(\prescript{g}{}{\mathbf{\hat{p}}_{f,k}^+}, \prescript{g}{}{\mathbf{\hat{v}}_{f,k}^+}, \prescript{g}{}{\mathbf{\tilde{a}}_k}) \hfill \\ \mathbf{f}_3(\mathbf{\hat{b}}_{a_{f,k}}^+, \mathbf{\hat{b}}_{\omega_{f,k}}^+, \mathbf{\hat{s}}_{a_{f,k}}^+) \hfill \end{bmatrix} \end{align*}
with its Jacobian \( F_k \) which is the first order approximation of the state transition function at the state \( \mathbf{\hat{x}}_{f,k}^+ \) and defined as
\begin{align*} \mathbf{F}_{k+1} =& \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \Bigg|_{\mathbf{\hat{x}}_{f,k}^+} \\[1em] =& \begin{bmatrix} \frac{\partial \mathbf{f}_1}{\partial \mathbf{x}} \\ \frac{\partial \mathbf{f}_2}{\partial \mathbf{x}} \\ \frac{\partial \mathbf{f}_3}{\partial \mathbf{x}} \end{bmatrix}_{\mathbf{\hat{x}}_{f,k}^+} = \begin{bmatrix} \mathbf{F}_{1_{k+1}} \\ \mathbf{F}_{2_{k+1}} \\ \mathbf{F}_{3_{k+1}} \end{bmatrix} \end{align*}
The quaternion integration for a constant angular velocity is done similarly to the previous parts, only the new angular velocity \( \boldsymbol{\tilde{\omega}}_k \) is used.
The quaternion prediction is given by
\begin{align*} \mathbf{\hat q}_{f,k+1}^- &= \mathbf{f}_1(\mathbf{\hat{q}}_{f,k}^+, \prescript{s}{}{\boldsymbol{\tilde{\omega}}_k}) \\[1em] &= \left[ \cos \left( \frac{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}|| \Delta t}{2}\right) I_4 + \frac{1}{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}||} \sin \left( \frac{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}|| \Delta t}{2} \right) \boldsymbol\Omega(\prescript{s}{}{\tilde{\boldsymbol\omega}_k}) \right] \mathbf{\hat q}_k, \end{align*}
with the operator matrix \( \boldsymbol\Omega(\prescript{s}{}{\boldsymbol\omega}) \) defined as
\[ \Omega(\prescript{s}{}{\tilde{\boldsymbol\omega}}) = \begin{bmatrix} 0 & -\prescript{s}{}{{\tilde{\boldsymbol\omega}}}^T \\ \prescript{s}{}{{\tilde{\boldsymbol\omega}}} & - \lfloor \prescript{s}{}{{\tilde{\boldsymbol\omega}}} \rfloor_{\times} \end{bmatrix} = \begin{bmatrix} 0 & -\prescript{s}{}{\tilde{\omega}_x} & -\prescript{s}{}{\tilde{\omega}_y} & -\prescript{s}{}{\tilde{\omega}_z} \\ \prescript{s}{}{\tilde{\omega}_x} & 0 & \prescript{s}{}{\tilde{\omega}_z} & -\prescript{s}{}{\tilde{\omega}_y} \\ \prescript{s}{}{\tilde{\omega}_y} & -\prescript{s}{}{\tilde{\omega}_z} & 0 & \prescript{s}{}{\tilde{\omega}_x} \\ \prescript{s}{}{\tilde{\omega}_z} & \prescript{s}{}{\tilde{\omega}_y} & -\prescript{s}{}{\tilde{\omega}_x} & 0 \end{bmatrix}. \]
\[ \mathbf{q}_{k+1} = \mathbf{q}_k \otimes \mathbf{q}\{\prescript{s}{}{\tilde{\boldsymbol\omega}_k} \Delta T\} \]
where\[ \mathbf{q}\{\prescript{s}{}{\tilde{\boldsymbol\omega}_k} \Delta T\} = \begin{bmatrix} \cos \left( \frac{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}|| \Delta T}{2} \right) \\ \sin \left( \frac{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}|| \Delta T}{2} \right) \frac{\prescript{s}{}{\tilde{\boldsymbol\omega}_k}}{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}||} \end{bmatrix}. \]
The Jacobian of the quaternion integration is then given by
\begin{align*} \mathbf{F}_{1_{k+1}} &= \frac{\partial \mathbf{f}_1}{\partial \mathbf{x}} \Bigg|_{\mathbf{\hat{x}}_{f,k}^+} \\[3mm] &= \begin{bmatrix} \mathbf{F}_{1,1} & \mathbf{0}_{4 \times 15} \end{bmatrix}, \end{align*}
with its component
\[ \mathbf{F}_{1,1_{k+1}} = \frac{1}{|| \prescript{s}{}{\tilde{\boldsymbol\omega}_k} ||} \begin{bmatrix} \alpha & -\prescript{s}{}{\tilde{\omega}}_{x_k} \beta & -\prescript{s}{}{\tilde{\omega}}_{y_k} \beta & -\prescript{s}{}{\tilde{\omega}}_{z_k} \beta \\ \prescript{s}{}{\tilde{\omega}}_{x_k} \beta & \alpha & \prescript{s}{}{\tilde{\omega}}_{z_k} \beta & -\prescript{s}{}{\tilde{\omega}}_{y_k} \beta \\ \prescript{s}{}{\tilde{\omega}}_{y_k} \beta & -\prescript{s}{}{\tilde{\omega}}_{z_k} \beta & \alpha & \prescript{s}{}{\tilde{\omega}}_{x_k} \beta \\ \prescript{s}{}{\tilde{\omega}}_{z_k} \beta & \prescript{s}{}{\tilde{\omega}}_{y_k} \beta & -\prescript{s}{}{\tilde{\omega}}_{x_k} \beta & \alpha \end{bmatrix} \]
where
\begin{align*} \alpha &= \cos \left( \frac{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}|| \Delta t}{2} \right) \ ||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}||, \\[3mm] \beta &= \sin \left( \frac{||\prescript{s}{}{\tilde{\boldsymbol\omega}_k}|| \Delta t}{2} \right). \end{align*}
As the state prediction of the orientation is now based on \( \prescript{s}{}{\tilde{\boldsymbol\omega}_k} \), we need to define this preprocessed angular velocity.
In general we have to assume a constant angular velocity for the discrete time integration. However, the angular velocity is not not always constant between two samples. Therefore the best assumption for a constant angular velocity between two samples has to be made.
One possible approach is to use the midward integration. Here, the assumption is made that the mean angular velocity between two samples is approximately the same as the mean of the angular velocity at the two samples. There exist also other approaches (Sola, 2017, p. 46 ff.) which are not considered here.
The preprocessed angular velocity is then defined as
\[ \prescript{s}{}{\tilde{\boldsymbol\omega}_k} = \begin{cases} \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^T & \text{if zero acceleration is detected} \\[3mm] \frac{1}{2} \left( \prescript{s}{}{\boldsymbol\omega}_{k}^\text{(meas)} + \prescript{s}{}{\boldsymbol\omega}_{k + 1}^\text{(meas)} \right) - \mathbf{\hat{b}}_{\omega_{k-1}}^+ & \text{otherwise} \end{cases} \]
The state prediction for the position, velocity and acceleration is done similarly to the previous parts. The only difference is that the acceleration is now based directly on the measurement. The state transition function for the position, velocity and preprocessed accelerations is then given by
\begin{align*} \begin{bmatrix} \prescript{g}{}{\mathbf{\hat a}}_{k+1} \\ \prescript{g}{}{\mathbf{\hat v}}_{k+1} \\ \prescript{g}{}{\mathbf{\hat p}}_{k+1} \end{bmatrix} &= \mathbf{f}_2 \left( \prescript{g}{}{\mathbf{\hat p}}_k, \prescript{g}{}{\mathbf{\hat v}}_k, \prescript{g}{}{\mathbf{\tilde a}}_k \right) \\[3mm] &= \begin{bmatrix} \prescript{g}{}{\hat v_{x_k}} + \prescript{g}{}{{\tilde a}_{x_k}} \Delta t \\ \prescript{g}{}{\hat v_{y_k}} + \prescript{g}{}{{\tilde a}_{y_k}} \Delta t \\ \prescript{g}{}{\hat v_{z_k}} + \prescript{g}{}{{\tilde a}_{z_k}} \Delta t \\ \prescript{g}{}{\hat p_{x_k}} + \prescript{g}{}{\hat v_{x_k}} \Delta t + \frac{1}{2} \prescript{g}{}{{\tilde a}_{x_k}} \Delta t^2 \\ \prescript{g}{}{\hat p_{y_k}} + \prescript{g}{}{\hat v_{y_k}} \Delta t + \frac{1}{2} \prescript{g}{}{{\tilde a}_{y_k}} \Delta t^2 \\ \prescript{g}{}{\hat p_{z_k}} + \prescript{g}{}{\hat v_{z_k}} \Delta t + \frac{1}{2} \prescript{g}{}{{\tilde a}_{z_k}} \Delta t^2 \end{bmatrix}. \end{align*}
As the above equation is linear, the Jacobian is given by
\begin{align*} \mathbf{F}_{2_{k+1}} &= \frac{\partial \mathbf{f}_2}{\partial \mathbf{x}} \Bigg|_{\mathbf{\hat{x}}_{f,k}^+} \\[3mm] &= \begin{bmatrix} \mathbf{0}_{6 \times 4} & \mathbf{F}_{2,1} & \mathbf{0}_{6 \times 9} \end{bmatrix}, \end{align*}
with the component \( \mathbf{F}_{2,1} \) beeing a lower triangular matrix given by
\[ \mathbf{F}_{2,1} = \begin{bmatrix} 1 \\ 0 & 1 & & &\mathbf 0\\ 0 & 0 & 1 \\ \Delta t & 0 & 0 & 1 \\ 0 & \Delta t & 0 & 0 & 1 \\ 0 & 0 & \Delta t & 0 & 0 & 1 \end{bmatrix}. \]
Similar to the angular velocity, the acceleration is also based directly on the measurement. As the change in velocity is due to the mean acceleration between two samples, we need to define a value which is close to the mean acceleration between two samples. However, in contrast to the angular velocity, we also need to mitigate the effect of the gravitational acceleration. Therefore we want to:
Formally expressed, the preprocessed acceleration is defined as
\[ \prescript{g}{}{{\tilde{\mathbf{a}}}_k} = \begin{cases} \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^T & \text{if zero velocity is detected} \\[3mm] \frac{1}{2} \left( \prescript{g}{}{\mathbf{a}_k^*} + \prescript{g}{}{\mathbf{a}_{k+1}^*} \right) - \mathbf{g} & \text{otherwise} \end{cases} \]
where
\[ \mathbf{g} = \begin{bmatrix} 0 & 0 & -g \end{bmatrix}^T \approx \begin{bmatrix} 0 & 0 & -9.81 \end{bmatrix}^T \]
is the gravitational acceleration in the global frame, and \( \prescript{g}{}{\mathbf{a}_k^*} \) and \( \prescript{g}{}{\mathbf{a}_{k+1}^*} \) are the accelerations in the global frame with the gravitational acceleration at time \( k \) and \( k+1 \), respectively, given by
\begin{align*} \prescript{g}{}{\mathbf{a}_k^*} &= \prescript{g}{l}{\mathbf{R}_q ({\mathbf{\hat{q}}_k^+}}) \left( \prescript{l}{}{\mathbf{a}_k^\text{(meas)}} - \mathbf{\hat{b}}_{a_{k-1}}^+ \right) \oslash \mathbf{\hat{s}}_{a_{k-1}}^+\\[3mm] \prescript{g}{}{\mathbf{a}_{k+1}^*} &= \prescript{g}{l}{\mathbf{R}_q ({\mathbf{\hat{q}}_{k+1}^-}}) \left( \prescript{l}{}{\mathbf{a}_{k+1}^\text{(meas)}} - \mathbf{\hat{b}}_{a_{k-1}}^- \right) \oslash \mathbf{\hat{s}}_{a_{k-1}}^+, \end{align*}
where \( \oslash \) denotes the Hadamard division (element-wise division) and \( \prescript{g}{l}{\mathbf{R}}_q \) is the rotation matrix from the local to the global frame.
As in the previous parts, the bias and sensitivity are predicted based on the previous state. The state transition function for the bias and sensitivity is given by
\begin{align*} \begin{bmatrix} \mathbf{\hat b}_{a_{k+1}} \\ \mathbf{\hat b}_{\omega_{k+1}} \\ \mathbf{\hat s}_{a_{k+1}} \end{bmatrix} &= \mathbf{f}_4 (\mathbf{\hat b}_{a_k}, \mathbf{\hat b}_{\omega_k}, \mathbf{\hat s}_{a_k}) \\[3mm] &= \begin{bmatrix} \hat b_{a_{x_k}} & \hat b_{a_{y_k}} & \hat b_{a_{z_k}} & \hat b_{\omega_{x_k}} & \hat b_{\omega_{y_k}} & \hat b_{\omega_{z_k}} & \hat s_{a_{x_k}} & \hat s_{a_{y_k}} & \hat s_{a_{z_k}} \end{bmatrix}^T \end{align*}
with the Jacobian
\begin{align*} \mathbf{F}_3 &= \frac{\partial \mathbf{f}_3}{\partial \mathbf{x}} \Bigg|_{\mathbf{\hat{x}}_{f,k}^+} \\[3mm] &= \begin{bmatrix} \mathbf{0}_{9 \times 10} & \mathbf{I}_9 \end{bmatrix}. \end{align*}
There are several different measurements in the system. They can be grouped into three categories:
As in the previous parts, the measurement model is splitted into submodels. The complete measurement model is
\[ \mathbf{h}(\mathbf{x}) = \begin{bmatrix} \mathbf{h}_1(\mathbf{x}) \\ \mathbf{h}_2(\mathbf{x}) \\ \mathbf{h}_3(\mathbf{x}) \end{bmatrix} \]
with the Jacobian
\begin{align*} \mathbf{H} &= \frac{\partial \mathbf{h}}{\partial \mathbf{x}} \\[3mm] &= \begin{bmatrix} \mathbf{H}_1 \\ \mathbf{H}_2 \\ \mathbf{H}_3 \end{bmatrix}. \end{align*}
As previously shown, the acceleration measurement can be modeled as
\begin{align*} \prescript{s}{}{\mathbf{a}}^\text{(meas)} &= \mathbf{s}_a \circ \prescript{s}{}{\mathbf{a}} + \mathbf{b}_a + \mathbf{n}_a \\[1em] &= \mathbf{s}_a \circ \left (\prescript{s}{g}{\mathbf{R}_q(\mathbf{q})} \cdot (\mathbf{g} + \prescript{g}{}{\mathbf{a}}) \right) + \mathbf{b}_a + \mathbf{n}_a, \end{align*}
where
Therefore, the measurement model for the acceleration is given by
\begin{align*} \mathbf{h}_1(\mathbf{x}) &= \mathbf{s}_a \circ \left( \prescript{s}{g}{\mathbf{R}_q(\mathbf{q})} \cdot (\mathbf{g} + \prescript{g}{}{\mathbf{a}}) \right) + \mathbf{b}_a. \end{align*}
Since this update is only done during stance phases, the acceleration \( \prescript{g}{}{\mathbf{a}} \) is always zero. Therefore the measurement model simplifies to
\begin{align*} \mathbf{h}_1(\mathbf{x}) &= \mathbf{s}_a \circ \prescript{s}{g}{\mathbf{R}_q(\mathbf{q})} \cdot \mathbf{g} + \mathbf{b}_a. \end{align*}
The Jacobian of the acceleration measurement is then given by
\begin{align*} \mathbf{H}_{1_k} &= \frac{\partial \mathbf{h}_1}{\partial \mathbf{x}} \\[3mm] &= \begin{bmatrix} \mathbf{H}_{1,1} & \mathbf{0}_{3 \times 6} & \mathbf{I}_3 & \mathbf{0}_{3 \times 3} & \mathbf{H}_{1,2} \end{bmatrix}, \end{align*}
with the components
\begin{align*} \mathbf{H}_{1,1_k} &= \frac{\partial \mathbf{h}_1}{\partial \mathbf{q}} \\[3mm] &= \mathbf{s}_a \circ \left (2g \begin{bmatrix} q_2 & -q_3 & q_0 & -q_1 \\ -q_1 & -q_0 & -q_3 & -q_2 \\ -q_0 & q_1 & q_2 & -q_3 \end{bmatrix}\right), \end{align*}
and
\begin{align*} \mathbf{H}_{1,2} &= \frac{\partial \mathbf{h}_1}{\partial \mathbf{s}_a} \\[3mm] &= g \cdot \text{diag} \begin{pmatrix} 2 (q_0 q_2 - q_1 q_3) & -2 (q_0 q_1 + q_2 q_3) & -q_0^2 + q_1^2 + q_2^2 - q_3^2 \end{pmatrix}. \end{align*}
The following measurement model is used for the zero velocity update. Whenever zero velocity is detected, we know that the velocity, acceleration and angular velocity. We already dealed with the acceleration as it is set to zero in the prediction step if zero velocity is detected. The velocity can be set to zero directly. As the angular velocity is not part of the state vector anymore, we cannot set it to zero. However, we can set the bias of the gyroscope to the actual measurement as everything we measure must be equal to bias plus noise. The measurement model for the zero velocity update is then given by
\begin{align*} \mathbf{h}_2(\mathbf{x}) &= \begin{bmatrix} \prescript{g}{}{v_x} \\ \prescript{g}{}{v_y} \\ \prescript{g}{}{v_z} \\ b_{\omega_x} \\ b_{\omega_y} \\ b_{\omega_z} \end{bmatrix}. \end{align*}
The corresponding Jacobian is given by
\begin{align*} \mathbf{H}_{2_k} &= \frac{\partial \mathbf{h}_2}{\partial \mathbf{x}} \\[3mm] &= \begin{cases} \begin{bmatrix} \mathbf{0}_{3 \times 4} & \mathbf{I}_3 & \mathbf{0}_{3 \times 12} \\ \mathbf{0}_{3 \times 13} & \mathbf{I}_3 & \mathbf{0}_{3 \times 3} \end{bmatrix} & \text{if zero velocity is detected} \\[3mm] \mathbf{0}_{7 \times 22} & \text{otherwise} \end{cases} \end{align*}
If we have knowledge about the position in the global frame, eg because we know, where the turn point is located, we can use this information to correct the position estimation. Also, whenever zero velocity is detected, we can reset the z-position in the global frame.
The measurement model for the position is then given by
\begin{align*} \mathbf{h}_3(\mathbf{x}) &= \begin{bmatrix} \prescript{g}{}{p_x} \\ \prescript{g}{}{p_y} \\ \prescript{g}{}{p_z} \end{bmatrix}. \end{align*}
The Jacobian of the position measurement is then given by
\begin{align*} \mathbf{H}_{3_k} &= \frac{\partial \mathbf{h}_3}{\partial \mathbf{x}} \\[3mm] &= \begin{cases} \begin{bmatrix} \ & \mathbf{0} & \\ \mathbf{0}_{1 \times 9} & 1 & \mathbf{0}_{1 \times 9} \end{bmatrix} & \text{if zero velocity is detected (height reset)} \\[3mm] \begin{bmatrix} \mathbf{0}_{2 \times 7} & \mathbf{I}_2 & \mathbf{0}_{2 \times 10} \\ & \mathbf{0} & \end{bmatrix} & \text{if position in xy-plane is known} \\[3mm] \begin{bmatrix} \mathbf{0}_{3 \times 7} & \mathbf{I}_3 & \mathbf{0}_{3 \times 9} \end{bmatrix} & \text{if both} \\[3mm] \mathbf{0}_{3 \times 19} & \text{otherwise} \end{cases} \end{align*}
The measurement vector is then defined as
\[ \mathbf{z} = \begin{bmatrix} \prescript{s}{}{a}_x & \prescript{s}{}{a}_y & \prescript{s}{}{a}_z & \prescript{g}{}{v}_x & \prescript{g}{}{v}_y & \prescript{g}{}{v}_z & \prescript{s}{}{\omega}_x & \prescript{s}{}{\omega}_y & \prescript{s}{}{\omega}_z & \prescript{g}{}{p}_x & \prescript{g}{}{p}_y & \prescript{g}{}{p}_z \end{bmatrix}^T \]
and should be filled with measurements and pseudo-measurements as
\[ \mathbf{z} = \begin{bmatrix} \prescript{s}{}{a}_x^\text{(meas)} & \prescript{s}{}{a}_y^\text{(meas)} & \prescript{s}{}{a}_z^\text{(meas)} & 0 & 0 & 0 & \prescript{s}{}{\omega}_x^\text{(meas)} & \prescript{s}{}{\omega}_y^\text{(meas)} & \prescript{s}{}{\omega}_z^\text{(meas)} & p_x^\text{(pseuso)} & p_y^\text{(pseudo)} & p_{z_0} \end{bmatrix}^T. \]
When which measurement has an effect on the state estimation is toggled by the Jacobian of the measurement model. If the Jacobian is zero, the Kalman gain is zero and the measurement has no effect on the state estimation.
The process noise covariance matrix should be defined as a function of five parameters:
| Parameter | Name | Description |
|---|---|---|
| \(\sigma_\omega^\text{(process)}\) | angular velocity noise | reflects the uncertainty of the orientation estimation |
| \(\sigma_a^\text{(process)}\) | acceleration noise | reflects the uncertainty of the translational movement |
| \(\sigma_{b_a}^\text{(process)}\) | bias noise of the accelerometer | reflects the uncertainty of the accelerometer bias |
| \(\sigma_{b_\omega}^\text{(process)}\) | bias noise of the gyroscope | reflects the uncertainty of the gyroscope bias |
| \(\sigma_{s_a}^\text{(process)}\) | sensitivity noise of the accelerometer | reflects the uncertainty of the accelerometer sensitivity |
Since the physical processes are independent of each other, the process noise covariance matrix is a block diagonal matrix. The block diagonal matrix is then given by
\[ \mathbf{Q} = \begin{bmatrix} \mathbf{Q}_{q} & & && \mathbf{0} \\ & \mathbf{Q}_{a} & \\ & & \mathbf{Q}_{b_a} & \\ & & & \mathbf{Q}_{b_\omega} & \\ \mathbf{0} & & & & \mathbf{Q}_{s_a} \end{bmatrix}. \]
As derived in this part, the process noise of the orientation is given by
\[ \mathbf{Q}_{q_k} = \left( \frac{\Delta t \ \sigma_\omega^\text{(process)}}{2} \right)^2 \begin{bmatrix} 1 - q_{0_k}^2 & -q_{0_k} q_{1_k} & -q_{0_k} q_{2_k} & -q_{0_k} q_{3_k} \\ -q_{1_k} q_{0_k} & 1 - q_{1_k}^2 & -q_{1_k} q_{2_k} & -q_{1_k} q_{3_k} \\ -q_{0_k} q_{2_k} & -q_{1_k} q_{2_k} & 1 - q_{2_k}^2 & -q_{2_k} q_{3_k} \\ -q_{0_k} q_{3_k} & -q_{1_k} q_{3_k} & -q_{2_k} q_{3_k} & 1 - q_{3_k}^2 \end{bmatrix}. \]
The process noise of the translational movement is given by
\[ \mathbf{Q}_{a} = \begin{bmatrix} \Delta t^2 \\ 0 & \Delta t^2 &&& \ast\\ 0 & 0 & \Delta t^2 \\ \frac{\Delta t^3}{2} & 0 & 0 & \frac{\Delta t^4}{4} \\ 0 & \frac{\Delta t^3}{2} & 0 & 0 & \frac{\Delta t^4}{4} \\ 0 & 0 & \frac{\Delta t^3}{2} & 0 & 0 & \frac{\Delta t^4}{4} \end{bmatrix} \sigma_a^{\text{(process)}^2}, \]
where \( \ast \) denotes the symmetric part of the matrix.
The process noise of the bias of the accelerometer is given by
\[ \mathbf{Q}_{b_a} = \text{diag}\begin{pmatrix} \sigma_{b_a}^\text{(process)} & \sigma_{b_a}^\text{(process)} & \sigma_{b_a}^\text{(process)} \end{pmatrix}^{\circ 2}. \]
The process noise of the bias of the gyroscope is given by
\[ \mathbf{Q}_{b_\omega} = \text{diag}\begin{pmatrix} \sigma_{b_\omega}^\text{(process)} & \sigma_{b_\omega}^\text{(process)} & \sigma_{b_\omega}^\text{(process)} \end{pmatrix}^{\circ 2}. \]
The process noise of the sensitivity of the accelerometer is given by
\[ \mathbf{Q}_{s_a} = \text{diag}\begin{pmatrix} \sigma_{s_a}^\text{(process)} & \sigma_{s_a}^\text{(process)} & \sigma_{s_a}^\text{(process)} \end{pmatrix}^{\circ 2}. \]
The measurement noise covariance matrix is defined as
\[ R = \text{diag} \begin{pmatrix} \sigma_{\text{acc}_{x,y}}^2 & \sigma_{\text{acc}_{x,y}}^2 & \sigma_{\text{acc}_{z}}^2 & \sigma_{\text{ZUPT}}^2 & \sigma_{\text{ZUPT}}^2 & \sigma_{\text{ZUPT}}^2 & \sigma_{\text{gyr}}^2 & \sigma_{\text{gyr}}^2 & \sigma_{\text{gyr}}^2 & \sigma_{\text{pseudo}}^2 & \sigma_{\text{pseudo}}^2 & \sigma_{\text{ZUPT}}^2 \end{pmatrix}, \]
where
| Parameter | Name | Value |
|---|---|---|
| \(\sigma_{\text{acc}_{x,y}}^2\) | accelerometer noise (x and y axis) | \( 0.045 \ \text{m/s}^2 \) |
| \(\sigma_{\text{acc_z}}^2\) | accelerometer noise (z axis) | \( 0.07 \ \text{m/s}^2 \) |
| \(\sigma_\text{ZUPT}^2\) | ZUPT noise | t.b.d |
| \(\sigma_\text{gyr}^2\) | gyroscope noise | \( 0.071 ^\circ \text{/s}\) |
| \(\sigma_\text{pseudo}^2\) | pseudo-measurement noise | t.b.d |
This part is not yet defined and will be determined experimentally.
The initial state has to be defined according to the expected initial conditions. In the example code, the initial position, velocity and acceleration are set to zero, the initial orientation is based on the accelerometer measurements and the initial bias and sensitivity are set to the parameters coming from the parameter estimation algorithm.
The Matlab implementation is done in the file derivation9Ekf6D.m.
The filtered and smoothed state variables are shown in the following plots. This results were achieved without any pre-calibration, i.e. 6-faces calibration to estimate the bias and sensitivity of the sensors.
The following plots show the filtered and smoothed state variables without using any pseudo-measurements, i.e. only the initial position at \( \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^T \) is used. To see the results in full resolution, click here.
As you can see, the overall length walked is estimated to be 8.7 m (correct is about 9 m). The final position is about 0.45 m off the correct final position. This leads to an accuracy of the estimated stride length of about 4 cm. The 180 ° turn is a bit overestimated. There exist positive z-positions which would indicate a foot inside the floor which is not possible and therefore wrong. It is assumed that this is due to the high accelerations after the heel strike (loading response). The biases and the sensitivity are converging after about 2.5 s to a sensible value (ground truth unknown).
The following plots show the filtered and smoothed state variables with using the final position as pseudo-measurement. To see the results in full resolution, click here.
The overall length walked is estimated to be 8.82 m (correct is about 9 m). Due to the final position pseudo-measurement, the final position is now not off the correct final position anymore. The estimated stride length is now estimated to an accuracy of about 3 cm. Also the 180 ° turn is now estimated correctly. The z-positions are still wrong.
The following plots show the filtered and smoothed state variables with using the final position and the turn-point as pseudo-measurement. To see the results in full resolution, click here.
The overall length walked is now correctly estimated to be 9 m, which also leads to a correct estimation of the stride length. However, in the top view, there are unusual medio-lateral movements which don't look like circumduction but more like a filtering artifact. As no mocap data is available, it is not possible to say if this is correct or not. The z-positions are still wrong.
The 6D EKF is able to estimate the stride length with an accuracy of about 4 cm if no pseudo-measurements are used. By using the final position as pseudo-measurement, the accuracy can be increased to about 3 cm. By using the final position and the turn-point as pseudo-measurement, the accuracy can be increased to almost zero. The z-positions are wrong due to the high accelerations after the heel strike. The medio-lateral movements in the top view are unusual and might be a filtering artifact. However, without any ground truth, it is not possible to say if this is correct or not.
As this results are promising, the next steps are to:
