Inertial Sensor-Based Smoother for Gait Analysis

Abstract

An off-line smoother algorithm is proposed to estimate foot motion using an inertial sensor unit (three-axis gyroscopes and accelerometers) attached to a shoe. The smoother gives more accurate foot motion estimation than filter-based algorithms by using all of the sensor data instead of using the current sensor data. The algorithm consists of two parts. In the first part, a Kalman filter is used to obtain initial foot motion estimation. In the second part, the error in the initial estimation is compensated using a smoother, where the problem is formulated in the quadratic optimization problem. An efficient solution of the quadratic optimization problem is given using the sparse structure. Through experiments, it is shown that the proposed algorithm can estimate foot motion more accurately than a filter-based algorithm with reasonable computation time. In particular, there is significant improvement in the foot motion estimation when the foot is moving off the floor: the z-axis position error squared sum (total time: 3.47 s) when the foot is in the air is 0.0807 m² (Kalman filter) and 0.0020 m² (the proposed smoother).

1. Introduction

Gait analysis is the study of human walking [1–3]. It is helpful in the medical diagnosis and treatment of those diseases that affect the ability to walk.

There are many devices that can be used to perform gait analysis [4]. Mechanical goniometers are used to measure hip, knee and ankle angles. Force plates on the floor or on a shoe (such as the wearable force plate from Tec Gihan Co., Kyoto, Japan) are used to measure the vertical force beneath the foot [5]. Optical motion trackers (such as Vicon) are used to measure the movement of a leg and a foot. These sensor systems can be used independently or together to give necessary gait parameters, such as stride length, walking speed and ground reaction forces.

Probably, an optical motion tracker is the most popular gait analysis tool [6], since it provides a very accurate description of foot and leg motion. The disadvantages of an optical motion tracker are its high cost and limited walking range. You can only increased walking range by using additional cameras, which can be quite expensive for a long walking range.

An alternative technology for the optical motion tracker is the use of inertial sensors [7,8]. Although the accuracy of inertial sensor-based estimation is generally worse than that of the optical motion tracker, the inertial sensor approach does not have a walking range limitation, and the cost is generally cheaper. If inertial sensors (gyroscopes and accelerometers) are attached to a leg, the angles of leg joints can be estimated (see [9–11] and commercial products from INSENCO and Xsens). These angle-based systems are extended to estimate whole human body motion [12–14]. Theoretically, the position of a leg also can be estimated using the inertial navigation algorithm [15]. However, the position estimation error quickly diverges when low cost inertial sensors are used. If inertial sensors are attached to a foot, the position (in addition to angle estimation) can be estimated using an inertial navigation algorithm. Unlike in the leg case, we know that the foot touches the ground almost periodically during walking. When the foot touches the ground, the velocity of the foot becomes temporarily zero, and during this zero velocity interval, the velocity errors of the inertial navigation algorithm can be reset. This zero velocity updating significantly reduces the position error [16].

Estimation of foot movement by attaching inertial sensors to the foot has been extensively studied in the context of a pedestrian navigation system [17,18]. In the filter-based foot motion estimation, the position error increases as soon as the foot is off the ground, since there are no measurements when the foot is in the air. The position error decreases once the foot touches the ground from the zero velocity updating. Thus, in the filter-based foot motion estimation, the motion estimation is not accurate when the foot is moving, but the final position (after the zero velocity updating) becomes accurate. In the pedestrian navigation system, the final position of the foot (that is, the final position of a person) is the main concern and, thus, the filter-based estimation can be used.

On the other hand, foot motion trajectory is the main concern in gait analysis. The filter estimated position can be improved using the smoother. The filtering algorithm uses the measurement data up to the current time to compute the current estimation. On the other hand, the smoother uses the whole measurement data (including the future data) to compute the current estimation. In [19], a smoother algorithm is proposed based on the forward-backward filter, where a smoothing algorithm is applied for each walking step. The position after the zero velocity updating is used as the initial value of a backward filter. The smoothed result for each step is combined rather heuristically to produce total walking estimation.

In this paper, the smoothing problem is formulated as a global optimization problem, where the whole walking motion is estimated in a single optimization problem. The smoother algorithm is formulated as a quadratic optimization problem (motivated by [20]), where its problem size depends on the total walking time. Since the problem size becomes very large even for a few minutes of walking data, the efficient solution is given utilizing the sparse structure of the problem.

The remainder of the paper is organized as follows. In Section 2, a basic material is introduced defining the coordinate systems and the main variables. In Section 3, a Kalman filter to estimate the foot motion is given. In Section 4, a smoother algorithm to estimate the foot motion is formulated in the quadratic optimization problem. In Section 5, a solution to the quadratic optimization problem is given using the sparse structure of the problem. In Section 6, experiment results are given to verify the proposed algorithm. Concluding remarks are given in Section 7.

2. Problem Formulation

Two-coordinate frames (the body coordinate frame and the navigation coordinate frame) are defined as in Figure 1. The three axes of the body coordinate frame coincide with the three axes of the inertial sensor unit. The z-axis of the navigation coordinate frame coincides with the local gravitational direction. The choice of the x-axis of the navigation coordinate frame can be chosen arbitrarily, since the walking direction is not important in the gait analysis. The notation [p]_n ([p]b) is used to denote that a vector p is represented in the navigation (body) coordinate frame.

Figure 1.

Overview of the foot inertial sensor unit.

The position of the foot is defined by [r]_n ∈ R³, which is the origin of the body coordinate frame expressed in the navigation coordinate frame. Similarly, the velocity of the foot is denoted by [v]_n ∈ R³. The attitude of the foot is represented using a quaternion q ∈ R⁴, which represents the rotation relationship between the navigation coordinate frame and the body coordinate frame. The directional cosine matrix corresponding to quaternion q is denoted by C(q) ∈ SO(3).

The basic dynamic equations [21,22] of q, r and v are given by:

$$\begin{array}{l}\dot{q}=\frac{1}{2}\mathrm{\Omega }\left(\omega \right)q\hfill \\ \dot{v}=C^{\prime }\left(q\right)a_b\hfill \\ \mathit{\text{ṙ}}=v\hfill \end{array}$$ (1)

where ω = [ω_x ω_y ω_z ]′ is the angular velocity of the body coordinate frame with respect to the navigation coordinate frame. The symbol Ω(ω) is defined by:

$$\mathrm{\Omega }\left(\omega \right)\triangleq \left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right].$$

The external acceleration (acceleration related to the movement, excluding the gravitational acceleration) expressed in the body coordinate frame is denoted by a_b ∈ R³.

The goal of this paper is to estimate q, r and v. The estimated values are denoted by q̂, r̂ and v̂, respectively The inertial navigation algorithm [19,23] is used to estimate q, r and v, where the equations in Equation (1) are integrated to compute q̂, r̂ and v̂. Accelerometer output y_a ∈ R³ and gyroscope output y_g ∈ R³ are related to ω and a_b as follows:

$$\begin{array}{l}{y}_a=C\left(q\right)\tilde{g}+a_b+b_a+n_a\hfill \\ y_g=\omega +b_g+n_g\hfill \end{array}$$ (2)

where n_a ∈ R³ and n_g ∈ R³ are sensor noises. The vector g̃ is the local gravitational acceleration, where g̃ = [0 0 9.8] is used. The sensor biases are denoted by b_a ∈ R³ and b_g ∈ R³. It is assumed that the sensor biases are constant and unknown.

Let T be the sampling period of the sensor output. The sampled outputs of y_a and y_g are denoted by y_a,k = y_a(kT) and y_g,k = y_g(kT). The same notation is used for q_k, r_k and v_k. Let N be the final discrete time index of the sensor output, where the starting index is one. The discrete sensor noise n_a,k and n_g,k are assumed to be zero mean white Gaussian noises, whose covariances are given by:

$$\begin{array}{cc}{R}_a=E\left\{n_{a,k}{n}_{a,k}^{\prime }\right\}\in R^{3\times 3},& R_g=E\left\{n_{g,k}{n}_{g,k}^{\prime }\right\}\in R^{3\times 3}\end{array}.$$ (3)

Due to high sensor noise level of low cost inertial sensors and unknown biases, an inertial navigation algorithm without external reference measurement can estimate foot motion only for a few seconds. To overcome this problem, a zero velocity updating method is used in an inertial sensor-based foot motion estimation. During walking, the foot touches the ground almost periodically. When the foot is on the ground, we know that the velocity of the foot is zero. Thus, whenever the foot touches the ground, the velocity error in the inertial navigation algorithm can be reset, which significantly reduces foot motion estimation errors. To use this zero velocity updating, zero velocity intervals (that is, when the foot is on the ground) must be detected. Many zero velocity detection algorithms have been proposed [16,24].

In this paper, we use a simple zero velocity detection algorithm. Let Z_m be a set of all discrete time indices belonging to the zero velocity intervals. The discrete time k belongs to Z_m if the following conditions are satisfied:

$$\begin{array}{ll}\Vert y_{g,i}\Vert \le B_g,\hfill & k-\frac{N_g}{2}\le i\le k+\frac{N_g}{2}\hfill \\ \Vert y_{a,i}-y_{a_{i-1}}\Vert \le B_a,\hfill & k-\frac{N_a}{2}\le i\le k+\frac{N_a}{2}\hfill \end{array}$$ (4)

where N_g and N_a are even number integers.

3. Kalman Filter

In this section, q, r, v, b_g and b_a are estimated using a Kalman filter. The estimated values are denoted by q̂_KF, r̂_KF, v̂_KF, b̂_g,KF and b̂_a,KF. As in [19,23], q_k ∼ b_a,k are not directly estimated in the Kalman filter. They are first estimated from the integration equation of (1), and then, the errors in the integrated values are estimated in the Kalman filter. These estimated values will be used in Section 4 to derive a smoothed estimation.

3.1. Initialization

Since only the relative position is important in the gait analysis, the initial position is assumed to be zero: that is, r̂_kf,1 = 0_3×1, where “1” denotes the initial discrete time index. Assuming that a person is not moving at the initial time, the initial attitude is computed using the triad algorithm [25,26]. The following two vector relations are used in the triad algorithm:

$$\begin{array}{c}{\left[y_a\right]}_b=C({\widehat{q}}_{KF,1})\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\\ \left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=C({\widehat{q}}_{KF,1})\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right].\end{array}$$ (5)

The pitch and roll angles are determined from the first vector relation, and the yaw angle is determined from the second vector relation. Since the heading of walking is not important in the gait analysis, the yaw angle can be chosen arbitrarily.

The initial velocity is chosen to be zero: that is, v̂_KF,1 = 0_3×1. If there is no particular information on b_g and b_a, the initial estimates are chosen to be zero (b̂_g,KF,1 = b̂_a,KF,1 = 0_3×1).

3.2. Integration of Estimated Values

Suppose we have computed q̂_KF,k, r̂_KF,k, ⋯ , b̂_{a, KF,k} at the discrete time k. The estimated values at the discrete time k + 1 are firstly computed using Equation (1), where ω and a_b are replaced by y_g − b̂_g and y_a − C(q̂)g̃ − b̂_a:

$$\begin{array}{l}\dot{\widehat{q}}=\frac{1}{2}\mathrm{\Omega }\left(y_a-{\widehat{b}}_a\right)\widehat{q}=\frac{1}{2}\mathrm{\Omega }\left(\omega +n_g-{\widehat{b}}_g\right)\widehat{q}\hfill \\ \dot{\widehat{v}}=C{\left(\widehat{q}\right)}^{\prime }\left(y_a-{\widehat{b}}_a\right)-\tilde{g}=C{\left(\widehat{q}\right)}^{\prime }\left(a_b+n_a+b_a-{\widehat{b}}_a\right)+\left(C{\left(\widehat{q}\right)}^{\prime }C\left(\widehat{q}\right)-I\right)\tilde{g}\hfill \\ \dot{\widehat{r}}=\widehat{v}.\hfill \end{array}$$ (6)

For later use, we define a function f_k, which represents a numerical integration of Equation (6) from kT to (k + 1)T:

$$\left[\begin{array}{c}{\widehat{q}}_{KF,k+1}\\ {\widehat{r}}_{KF,k+1}\\ {\widehat{v}}_{KF,k+1}\end{array}\right]=f_k\left(\left[\begin{array}{c}{\widehat{q}}_{KF,k}\\ {\widehat{r}}_{KF,k}\\ {\widehat{v}}_{KF,k}\end{array}\right],\left[\begin{array}{c}{n}_{g,k}\\ n_{a,k}\end{array}\right],\left[\begin{array}{c}{b}_g-{\widehat{b}}_{g,k}\\ b_a-{\widehat{b}}_{a,k}\end{array}\right]\right).$$ (7)

We assume that the variables not used as arguments of f_k are embedded in f_k: for example, the ω, C(q) and g̃ terms are assumed to be embedded in the function f_k.

3.3. Kalman Filter

Due to noise terms (n_g,k and n_a,k) and bias estimation error (b_g − b̂_g,k and b_a − b̂_a,k), there are errors in the numerically-integrated values of Equation (7). These errors are estimated using a Kalman filter. Let X_KF,k be defined by:

$$X_{KF,k}=\left[\begin{array}{c}{\overline{q}}_{KF,k}\\ {\overline{r}}_{KF,k}\\ {\overline{v}}_{KF,k}\\ {\overline{b}}_{g,KF,k}\\ {\overline{b}}_{a,KF,k}\end{array}\right]=\left[\begin{array}{c}\left[\begin{array}{cc}{0}_{3\times 1}& I_3\end{array}\right]\left({\widehat{q}}_{KF,k}^{*}\otimes q_k\right)\\ r_k-{\widehat{r}}_{KF,k}\\ v_k-{\widehat{v}}_{KF,k}\\ b_g-{\widehat{b}}_{g,KF,k}\\ b_a-{\widehat{b}}_{a,KF,k}\end{array}\right]\in \left[\begin{array}{c}{R}^3\\ R^3\\ R^3\\ R^3\\ R^3\end{array}\right]$$ (8)

where ⊗ is the quaternion multiplication and q* denotes the quaternion conjugate of a quaternion q [21]. The elements of X_KF,k represent the estimation errors in q̂_KF,k ∼ b̂_a,KF,k. In defining X_KF,k, it is assumed that the quaternion errors in q̂_KF,k are small. Thus, (q̂_KF,k)* ⊗ q_k is approximated by:

$$\left({\widehat{q}}_{KF,k}\right)*\otimes q_k\approx \left[\begin{array}{c}1\\ {\overline{q}}_{KF,k}\end{array}\right]\in \left[\begin{array}{c}R\\ R^3\end{array}\right].$$

With this assumption, the error in q̂_KF,k ∈ R⁴ is represented by three-dimensional vector q̄_KF,k instead of a four-dimensional vector. See [27] for the details about the multiplicative error notation of a quaternion. The initial estimate is set to X̂_KF,1 = 0_15×1, and the initial error covariance P₁ is given by:

$$\begin{array}{c}{P}_1=E\left\{\left(X_{KF,1}-{\widehat{X}}_{KF,1}\right){\left(X_{KF,1}-{\widehat{X}}_{KF,1}\right)}^{\prime }\right\}\\ =\left[\begin{array}{ccccc}{P}_{q,\mathit{\text{init}}}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& P_{r,\mathit{\text{init}}}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& P_{v,\mathit{\text{init}}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& P_{b_{g,\mathit{\text{init}}}}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& P_{b_{a,\mathit{\text{init}}}}\end{array}\right]\end{array}.$$ (9)

The initial attitude error covariance P_q,init is given by the algorithm in [25], where R_a and 10⁻¹¹I₃ are used as measurement noise covariances of the first and second equations in Equation (5). The small covariance value for the second equation is to make P_q,init nonsingular, since $P_{q,\mathit{\text{init}}}^{-1}$

is used in the smoothing algorithm. The remaining covariances P_r,init ∼ P_ba,init can be considered as design parameters, whose values are given in Table 1.

Table 1.

Parameters of the proposed smoother and Kalman filter.

Parameter	Symbol	Values	Related Equation
sampling period	T	0.01
zero velocity detection parameters	Bg, Ng, Ba, Na	0.2, 10, 0.4, 10	(4)
sensor noise error covariance	Rg, Ra	0.001I3, 0.01I3
measurement noise covariance	Rv, Rr	0.0001I3,0.0001	(15)
filter and smoother parameter	Pr,init, Pv,init	0.0001I3, 0.0001I3	(9), (27)
filter and smoother parameter	Pbg,init, Pba,init	0.000001I3, 0.000001I3	(9), (27)

The dynamic equation of X_KF,k is given by:

$$X_{KF,k+1}=\left[\begin{array}{cc}{\mathrm{\Phi }}_{k+1,k}& {\mathrm{\Psi }}_{k+1,k}\\ 0_{9\times 6}& I_6\end{array}\right]X_{KF,k}+\left[\frac{w_k}{0_{6\times 1}}\right]$$ (10)

where:

$$e^{A_{k}T}=\left[\begin{array}{cc}{\mathrm{\Phi }}_{k+1,k}& {\mathrm{\Psi }}_{k+1,k}\\ 0_{9\times 6}& I_6\end{array}\right]\in [\begin{array}{cc}{R}^{9\times 9}& R^{9\times 6}\\ R^{6\times 9}& R^{6\times 6}\end{array}]$$ (11)

$$A_k=\left[\begin{array}{cc}\begin{array}{ccc}-\left[y_{a,k}\times \right]& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_3\\ -2C{\left({\widehat{q}}_{KF,k}\right)}^{\prime }\left[y_{a,k}\times \right]& 0_{3\times 3}& 0_{3\times 3}\end{array}& \begin{array}{cc}-\frac{1}{2}{I}_3& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& -C{\left({\widehat{q}}_{KF,k}\right)}^{\prime }\end{array}\\ \begin{array}{ccc}{0}_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}\end{array}& \begin{array}{cc}{0}_{6\times 3}& 0_{6\times 3}\end{array}\end{array}\right]=\left[\begin{array}{cc}{A}_{k,1}& A_{k,2}\\ 0_{6\times 9}& 0_{6\times 6}\end{array}\right].$$

For a vector p = [p₁ p₂ p₃]′ ∈ R³, [p×] is defined by:

$$\left[p\times \right]=\left[\begin{array}{ccc}0& -p_3& p_2\\ p_3& 0& -p_1\\ -p_2& p_1& 0\end{array}\right].$$

Note that A_k is a lower triangular matrix, and thus, e^AkT is also a lower triangular matrix having the structure in Equation (11) [28].

The process noise w_k is the zero mean white Gaussian noise with:

$$Q_k=E\left\{w_k{w}_k^{\prime }\right\}={\mathit{\int }}_0^{T}exp\left(A_{k,1}r\right)\left[\begin{array}{ccc}\frac{1}{4}{R}_g& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {10}^{-6}{I}_3& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& R_a\end{array}\right]exp\left(A_{k,1}^{\prime }r\right)dr$$

where the 10⁻⁶ value in Q_k is added to make Q_k nonsingular, since $Q_k^{-1}$ is used in the smoother. Derivation of A_k and Q_k can be obtained by slightly modifying the results in [19,23]. The computational load of e^AkT can be reduced by using the following approximation:

$$e^{A_{k}T}\approx I_{15}+A_{k}T+\frac{1}{2!}{A}_k^2{T}^2.$$ (12)

Remark 1

How the estimation error in k-th discrete time evolves after the integration Equation (7) is represented in Equation (10). For example, (10) states that the error in r̂_KF,k+1 comes from two sources. One is from the estimation error in k-th time (before the integration), and the other is from the process of numerical integration. The latter is not from the numerical integration error (it is ignored), but from the noise terms and bias estimation errors.

In an inertial sensor-only foot motion estimation, there is no external measurement. The fact that the velocity is zero during the zero velocity intervals is used as a fictitious measurement.

During the zero velocity interval, the following equation can be used as a measurement equation:

$$0_{3\times 1}-{\widehat{v}}_{KF,k}=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& I_3& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]X_{KF,k}+n_{r,k},k\in Z_m$$ (13)

where n_v,k is a fictitious measurement noise representing the confidence of zero velocity interval detection in Equation (4). The z-axis value of r̂_KF,k is almost the same when the foot is on the ground, assuming that a person is walking on a flat floor. Thus, the following measurement equation can also be used during the zero velocity interval.

$$0-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\widehat{r}}_{KF,k}=\left[\begin{array}{ccccc}{0}_{1\times 3}& \begin{array}{ccc}0& 0& 1\end{array}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}\end{array}\right]X_{KF,k}+n_{r,k},k\in Z_m$$ (14)

where n_r,k is a fictitious measurement noise.

We assume that n_v,k and n_r,k are zero mean white Gaussian noises, where covariances are given by:

$$\begin{array}{cc}{R}_v=\text{E}\left\{n_{v,k}{n}_{v,k}^{\prime }\right\}\in R^{3\times 3},& R_r=\text{E}\left\{n_{r,k}{n}_{r,k}^{\prime }\right\}\in R.\end{array}$$ (15)

A standard discrete time Kalman filter is applied to Equations (10), (13) and (14). If k ∈ Z_m, we can compute X̂_KF,k from the Kalman filter, and these values are used to update q̂_KF,k ∼ b∈_a,KF,k.

The Kalman filter algorithm of position estimation (r̂_KF,k) is summarized in the following for k₁ ≤ k ≤ k₂ assuming that k₁ ∈ Z_m, k₂ ∈ Z_m and k ∉ Z_m for k₁ < k < k₂. The algorithm for other values (q̂_KF,k, v̂_KF,k, b̂_g,KF,k and b̂_a,KF,k) are similar.

• discrete time k = k₁ ∈ Z_m

  • –
    r̂_KF,k is given from the previous step
  • –
    r̄̂_KF,k = 0
• discrete time k₁ < k < k₂

  • –
    compute r̂_KF,k using Equation (7)
  • –
    r̄̂_KF,k−1 = 0 from Kalman filter time update Equation (10) with r̄̂_KF,k−1 = 0
• discrete time k = k₂ ∈ Z_m

  • –
    compute r̂_KF,k using Equation (7)
  • –
    compute r̄̂_KF,k from the Kalman filter measurement update Equations (13) and (14)
  • –
    r̂_KF,k ⟸ r̂_KF,k + r̄̂_KF,k
  • –
    r̄̂_KF,k−1 = 0

The result in this section is not a new result, and similar results can be found in [19].

4. Smoothing Algorithm

In this section, the Kalman filter-estimated values (q̂_KF,k ∼ b̂_a,KF,k) in Section 3 are improved using the smoother, where the errors in q̂_KF,k, r̂_KF,k, v̂_KF,k, b̂_g,KF and b̂_a,KF are compensated for. To do that, the estimation error is defined as follows.

$$\begin{array}{l}{\overline{q}}_{SM,k}=\left[\begin{array}{cc}{0}_{3\times 1}& I_3\end{array}\right]{\widehat{q}}_{KF,k}^{*}\otimes q_k\\ {\overline{r}}_{SM,k}=r_k-{\widehat{r}}_{KF,k}\\ {\overline{v}}_{SM,k}=v_k-{\widehat{v}}_{KF,k}\\ {\overline{b}}_{g,SM}=b_g-{\widehat{b}}_{g,KF}\\ {\overline{b}}_{a,SM}=b_a-{\widehat{b}}_{a,KF}\end{array}$$ (16)

where b̂_g,KF and b̂_a,KF are b̂_g,KF,N and b̂_a,KF,N (the final values) in the Kalman filter, which can be considered as the most accurate estimation of b_g and b_a. Since b_g and b_a are constant, b̄_g,SM and b̄_a,SM are not time dependent, while q̄_SM,k, r̄_SM,k and v̄_SM,k are time dependent. We note that the estimation errors in Equation (8) represent the errors before the measurement update, while the errors in Equation (16) represent the errors after the measurement update.

Let X_SM,k be defined by:

$$X_{SM,k}=\left[\begin{array}{c}{\overline{q}}_{SM,k}\\ {\overline{r}}_{SM,k}\\ {\overline{v}}_{SM,k}\end{array}\right].$$ (17)

Equations for time-dependent variables (q̄_SM,k, r̄_SM,k and v̄_SM,k) are given by:

$${\zeta }_k+X_{SM,k+1}={\mathrm{\Phi }}_{k+1,k}{X}_{SM,k}+{\mathrm{\psi }}_{k+1,k}\left[\begin{array}{c}{\overline{b}}_{g,SM}\\ {\overline{b}}_{a,SM}\end{array}\right]+w_k$$ (18)

where Φ and Ψ are defined in Equation (11) and:

$${\zeta }_k=\left[\begin{array}{c}{\tilde{q}}_{SM,k}\\ {\widehat{r}}_{KF,k+1}-f_{2,k}\\ {\widehat{v}}_{KF,k+1}-f_{3,k}\end{array}\right],$$ (19)

$$\begin{array}{c}\left[\begin{array}{c}{f}_{k,1}\\ f_{k,2}\\ f_{k,3}\end{array}\right]=f_k\left(\left[\begin{array}{c}{\widehat{q}}_{KF,k}\\ {\widehat{r}}_{KF,k}\\ {\widehat{v}}_{KF,k}\end{array}\right],\left[\begin{array}{c}{n}_{g,k}\\ n_{a,k}\end{array}\right],\left[\begin{array}{c}{\overline{b}}_{g,SM}\\ {\overline{b}}_{a,SM}\end{array}\right]\right),\\ {\tilde{q}}_{SM,k}=\left[\begin{array}{cc}{0}_{3\times 1}& I_3\end{array}\right]f_{1,k}^{*}\otimes {\widehat{q}}_{KF,k+1}.\end{array}$$ (20)

Derivation of Equation (18) is now explained. From Equations (16), (19) and (20), we have:

$${\zeta }_k+X_{SM,k+1}=\left[\begin{array}{c}\left[\begin{array}{cc}{0}_{3\times 3}& I_3\end{array}\right]f_{1,k}^{*}\otimes q_{k+1}\\ r_{k+1}-f_{2,k}\\ v_{k+1}-f_{3,k}\end{array}\right]\in R^9.$$ (21)

The second and the third elements of Equation (21) are straightforward. The first element of Equation (21) is derived from the following:

$$\begin{array}{ll}{q}_{k+1}& ={\widehat{q}}_{KF,k+1}\otimes \left[\begin{array}{c}1\\ {\overline{q}}_{SM,k+1}\end{array}\right]\\ & =f_{1,k}\otimes f_{1,k}^{*}\otimes {\widehat{q}}_{KF,k+1}\otimes \left[\begin{array}{c}1\\ {\overline{q}}_{SM,k+1}\end{array}\right]\\ & \approx f_{1,k}\otimes \left[\begin{array}{c}1\\ {\tilde{q}}_{SM,k}+{\overline{q}}_{SM,k+1}\end{array}\right]\end{array}$$ (22)

where the following approximation is used:

$$f_{1,k}^{*}\otimes {\widehat{q}}_{KF,k+1}\approx \left[\begin{array}{c}1\\ {\tilde{q}}_{SM,k+1}\end{array}\right],$$ (23)

$$\left[\begin{array}{c}1\\ {\tilde{q}}_{SM,k}\end{array}\right]\otimes \left[\begin{array}{c}1\\ {\overline{q}}_{SM,k+1}\end{array}\right]\approx \left[\begin{array}{c}1\\ {\tilde{q}}_{SM,k}+{\overline{q}}_{SM,k+1}\end{array}\right].$$ (24)

From Equation (21), ξ_k + X_SM,k+1 denotes the estimation errors of f_k in Equation (20). Thus, Equation (18) represents how the estimation error evolves after the integration Equation (20). Thus, Equation (18) is essentially the same as Equation (10). In the exact same methods in the case of Equation (10), Equation (21) can be obtained by slightly modifying the results in [19,23].

The smoothing problem is formulated as a quadratic optimization problem using the method in [20]. Let an optimization variable X̃ be defined by:

$$\tilde{X}=\left[\frac{\begin{array}{c}{X}_{SM,1}\\ X_{SM,2}\\ ⋮\\ X_{SM,N}\end{array}}{\begin{array}{c}{\overline{b}}_{g,SM}\\ {\overline{b}}_{a,SM}\end{array}}\right]\in \left[\frac{R^{9\times N}}{R^6}\right].$$ (25)

The smoothing problem can be formulated as the following quadratic optimization problem:

$$\underset{\tilde{X}}{min}J\left(\tilde{X}\right)$$ (26)

where:

$$\begin{array}{c}J\left(\tilde{X}\right)=\frac{1}{2}{\text{∑}}_{k=1}^{N-1}{w}_k^{\prime }{Q}_k^{-1}{w}_k+\frac{1}{2}{\text{∑}}_{k\in Z_m}{\left(z_k-{\tilde{H}}_k{X}_{SM,k}\right)}^{\prime }{\tilde{R}}^{-1}\left(z_k-{\tilde{H}}_k{X}_{SM,k}\right)\\ +\frac{1}{2}{\left(X_{SM,1}-X_{\mathit{\text{init}}}\right)}^{\prime }{P}_{X_{\mathit{\text{init}}}}^{-1}\left(X_{SM,1}-X_{\mathit{\text{init}}}\right)\\ +\frac{1}{2}{\left({\widehat{b}}_{g,SM}+{\overline{b}}_{g,SM}-b_{g,\mathit{\text{init}}}\right)}^{\prime }{P}_{b_{g,\mathit{\text{init}}}}^{-1}\left({\widehat{b}}_{g,SM}+{\overline{b}}_{g,SM}-b_{g,\mathit{\text{init}}}\right)\\ +\frac{1}{2}{\left({\widehat{b}}_{a,SM}+{\overline{b}}_{a,SM}-b_{a,\mathit{\text{init}}}\right)}^{\prime }{P}_{b_{a,\mathit{\text{init}}}}^{-1}\left({\widehat{b}}_{a,SM}+{\overline{b}}_{a,SM}-b_{a,\mathit{\text{init}}}\right)\end{array}$$ (27)

$$X_{\mathit{\text{init}}}=\left[\begin{array}{c}{\widehat{q}}_{KF,1}\\ 0_{3\times 3}\\ 0_{3\times 1}\end{array}\right],P_{X_{\mathit{\text{init}}}}=\left[\begin{array}{ccc}{P}_{q,\mathit{\text{init}}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& P_{r,\mathit{\text{init}}}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& P_{v,\mathit{\text{init}}}\end{array}\right]$$

$$z_k=\left[\begin{array}{c}{0}_{3\times 1}-{\widehat{v}}_{KF,k}\\ 0-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\widehat{r}}_{KF,k}\end{array}\right],{\tilde{H}}_k=\left[\begin{array}{ccc}{0}_{3\times 3}& 0_{3\times 3}& I_3\\ 0_{1\times 3}& \begin{array}{ccc}0& 0& 1\end{array}& 0_{1\times 3}\end{array}\right],\tilde{R}=\left[\begin{array}{cc}{R}_v& 0\\ 0& R_r\end{array}\right].$$

Initial information on b_g and b_a are reflected in b_g,init ∈ R³ and b_a,init ∈ R³. For example, b_g,init can be computed if a sensor unit is put on the ground without moving for 1∼2 min. If there is no information on b_g and b_a, we set b_g,init and b_a,init to zero.

Inserting Equation (18) into Equation (27), we have:

$$\begin{array}{c}J\left(\tilde{X}\right)=\frac{1}{2}{{\text{∑}}_{k=1}^{N-1}\left({\zeta }_k+X_{SM,k+1}-{\mathrm{\Phi }}_{k+1,k}{X}_{SM,k}-{\mathrm{\Psi }}_{k+1,k}\left[\begin{array}{c}{\overline{b}}_{g,SM}\\ {\overline{b}}_{a,SM}\end{array}\right]\right)}^{\prime }{Q}^{-1}\\ \left({\zeta }_k+X_{SM,k+1}-{\mathrm{\Phi }}_{k+1,k}{X}_{SM,k}-{\mathrm{\Psi }}_{k+1,k}\left[\begin{array}{c}{\overline{b}}_{g,SM}\\ {\overline{b}}_{a,SM}\end{array}\right]\right)\\ +\frac{1}{2}{\text{∑}}_{k\in Z_m}{\left(z_k-{\tilde{H}}_k{X}_{SM,k}\right)}^{\prime }{R}^{-1}\left(z_k-{\tilde{H}}_k{X}_{SM,k}\right)+\frac{1}{2}{\left(X_{SM,1}-X_{\mathit{\text{init}}}\right)}^{\prime }{P}_{X_{\mathit{\text{init}}}}^{-1}\left(X_{SM,1}-X_{\mathit{\text{init}}}\right)\\ +\frac{1}{2}{\left({\widehat{b}}_{g,SM}+{\overline{b}}_{g,SM}-b_{g,\mathit{\text{init}}}\right)}^{\prime }{P}_{b_{g,\mathit{\text{init}}}}^{-1}\left({\widehat{b}}_{g,SM}+{\overline{b}}_{g,SM}-b_{g,\mathit{\text{init}}}\right)\\ +\frac{1}{2}{\left({\widehat{b}}_{a,SM}+{\overline{b}}_{a,SM}-b_{a,\mathit{\text{init}}}\right)}^{\prime }{P}_{b_{a,\mathit{\text{init}}}}^{-1}\left({\widehat{b}}_{a,SM}+{\overline{b}}_{a,SM}-b_{a,\mathit{\text{init}}}\right).\end{array}$$ (28)

It is not difficult to see that Equation (28) is a quadratic function of X̃ ∈ R^9×N+6:

$$J\left(\tilde{X}\right)=\frac{1}{2}\tilde{X}\prime M_1\tilde{X}+M_2^{\prime }\tilde{X}+M_3$$ (29)

where M₁, M₂ and M₃ can be computed from Equation (28). M₁ and M₂ are given in the Appendix (M₃ is omitted, since it is irrelevant in the optimization). The minimizing solution of Equation (29) can be computed by solving the following linear equation:

$$M_1{\tilde{X}}^{*}+M_2=0$$ (30)

where X̃* is the minimizing solution.

Once the minimizing solution X̃* is computed, the smoother estimated values are computed using Equation (16). For example, the position estimate is computed as follows:

$${\widehat{r}}_{SM,k}={\widehat{r}}_{KF,k}+X_{SM,k}^{*}$$

where r̂_SM,k is the smoother estimate of r_k and $X_{SM,k}^{*}$ is the 9(k − 1) + 1 ∼ 9k rows of X̃*.

The result in this section is a new result. The main contribution of this section is that the foot motion Kalman filter error is compensated for using the quadratic optimization problem.

5. Smoothing Optimization Solution

Although the smoothing problem boils down to a simple linear Equation (30)M₁ ∈ R^{(9N+6)×(9N+6)} becomes very large when N is large. Suppose we solve Equation (30) using the Cholesky decomposition. Let L be the Cholesky triangle (lower triangular matrix) satisfying:

$$M_1=LL\prime .$$ (31)

Once L is computed, X̃* can be computed by the following equation:

$$\begin{array}{l}L\tilde{Y}=-M_2\hfill \\ L\prime {\tilde{X}}^{*}=\tilde{Y}.\hfill \end{array}$$ (32)

The computational load of Cholesky decomposition Equation (31) is (9N + 6)³/6 flops, and the load for each linear equation in Equation (32) is (9N + 6)²/6 flops [29]. Suppose T = 0.01 and N = 12,000 (2-min walk data), then the computational load of the Cholesky decomposition alone is approximately 2 × 10¹⁴ flops, which is practically difficult to compute (due to the very long computation time and large memory requirement).

Fortunately, the matrix M₁ has a sparse structure, and the computational load can be reduced using the sparse structure.

We partition M₁ and M₂ as follows:

$$M_1=\left[\frac{M_{11}}{M_{12}^{\prime }}|\frac{M_{12}}{M_{13}}\right]\in \left[\frac{R^{9N\times 9N}}{R^{6\times 9N}}|\frac{R^{9N\times 6}}{R^{6\times 6}}\right],M_2=\left[\frac{M_{21}}{M_{22}}\right]\in \left[\frac{R^{9N\times 1}}{R^{6\times 1}}\right].$$ (33)

We also partition X̃ in Equation (29) accordingly:

$$\tilde{X}=\left[\frac{{\tilde{X}}_1}{{\tilde{X}}_2}\right]\in \left[\frac{R^{9N\times 1}}{R^{6\times 1}}\right]$$ (34)

From the definition of M₁₁ in the Appendix, we can see that M₁₁ is a banded matrix [29] with the bandwidth p = 17:

$$M_{11}=\left[\begin{array}{ccccccc}*& *& & & & & \\ *& *& *& & & & \\ & *& *& *& & & \\ & & \ddots & \ddots & \ddots & & \\ & & & *& *& *& \\ & & & & *& *& *\\ & & & & & *& *\end{array}\right]$$ (35)

where * denotes the arbitrary 9 × 9 block matrix. All unspecified entries are zero 9 × 9 block matrices.

Equation (30) can be rewritten using the partitions in Equations (33) and (34):

$$\left[\frac{M_{11}}{M_{12}^{\prime }}|\frac{M_{12}}{M_{13}}\right]\left[\frac{{\tilde{X}}_1}{{\tilde{X}}_2}\right]=-\left[\frac{M_{21}}{M_{22}}\right]$$ (36)

Equation (36) can be transformed into the following equation using the Schur complement [30]:

$$\begin{array}{cc}\hfill (M_{13}-M_{12}^{\prime }{M}_{11}^{-1}{M}_{12}){\tilde{X}}_2& =M_{22}-M_{12}^{\prime }{M}_{11}^{-1}{M}_{21}\hfill \\ \hfill M_{11}{\tilde{X}}_1& =M_{21}-M_{12}{\tilde{X}}_2.\hfill \end{array}$$ (37)

The Equation (37) can be solved in the following sequence [30]. First, find the Cholesky decomposition of banded matrix M₁₁:

$$M_{11}=L_{11}{L}_{11}^{\prime }$$ (38)

where L₁₁ ∈ R^9N×9N is a lower triangular matrix. Find U₁ ∈ R^9N×1 and U₂ ∈ R^9N×1 from the following linear equation:

$$\begin{array}{l}{L}_{11}{U}_1=M_{12}\hfill \\ L_{11}{U}_2=M_{21}.\hfill \end{array}$$ (39)

Inserting Equation (39) into Equation (37), we have:

$$(M_{13}-U_1^{\prime }{U}_1){\tilde{X}}_2=M_{22}-U_1^{\prime }{U}_2.$$ (40)

After computing X̃₂ in Equation (40), we compute X̃₁ from the following equations:

$$\begin{array}{l}{L}_{11}{U}_3=M_{21}-M_{12}{\tilde{X}}_2\hfill \\ L_{11}^{\prime }{\tilde{X}}_1=U_3.\hfill \end{array}$$ (41)

When solving Equations (38)–(41), the algorithms for banded matrices in [29] can be used for efficient computation and memory usage. The computational load of the Cholesky decomposition of the banded matrix Equation (38) is (9N(p² + 3p))/2 flops (p = 17 is the bandwidth of M₁₁, and 9N ≫ p is assumed) and 9N square root computation. The computational load of each equation in Equations (39) and (41) is 9N(p + 1) flops. Equation (40) is a small dimensional problem (X̃₂ ∈ R⁶).

Compared with the standard method, the solution using the banded structure of M₁₁ requires much smaller computation and memory. For example, the Cholesky decomposition requires approximately 3.6 × 10⁷ flops and 9N square root computation in the case of N = 12, 000. Compared with 2 × 10¹⁴ flops in the standard computation case, its computational requirement is significantly small.

The main contribution of this section is to derive an efficient solution to the quadratic optimization problem and to investigate the quantitative computational requirement.

6. Experiment

Two experiments are performed to verify the proposed algorithm. In the experiments, an inertial sensor unit (Xsens MTi sensor unit) is attached on the foot, as shown in Figure 2. The parameters used in both experiments are given in Table 1.

Figure 2.

Experimental setup for the first experiment.

In the first experiment, a person walked three steps, while the movement of the foot is tracked using an optical motion tracking system (Optitrack six Flex 13 camera system). The experimental setup is given in Figure 2. Six optical markers are used in the optical tracking system, and the center position of six markers is used as the true foot position. Since the coordinate systems of the optical tracker and the inertial sensor unit are different, the optical tracker output is rotated and translated.

The estimated position (Kalman filter and the proposed smoother) with the flat floor assumption (see Equation (14)) is given in Figure 3: the left figure is the z-axis movement, and the right figure is the xy-axis position. In the left figure, the zero velocity intervals are represented by the “*” symbol.

Figure 3.

Estimated position with the flat floor assumption: z-axis position (Left) and xy-axis position (Right).

Assuming that the motion tracker value is the true position value, we can see that the Kalman filter gives a good final position estimation (around 5 s). However, the position estimated during walking is not accurate; for example, see the values between 2 and 3 s. Thus, the Kalman filter result may be good enough for the pedestrian navigation system, where the final position is important. However, the Kalman result is not good enough for gait analysis, where it is important to know how the foot has moved in addition to the final position. The final estimated position of the smoother is almost similar to that of the Kalman filter. However, we can see that there is significant improvement in the middle foot position estimation.

The z-axis estimation error is compared quantitatively in Table 2. The z-axis estimation error e_z,k is defined by:

$$e_{z,k}=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left(r_k-{\widehat{r}}_k\right)$$

where r̂_k is either r̂_k = r̂_KF,k or r̂_k = r̂_SM,k.

Table 2.

Estimated z position error squared sum with the flat floor assumption.

Algorithms	$E_1={\text{∑}}_{k\in Z_m}{e}_{z,k}^2$	$E_2={\text{∑}}_{k\notin Z_m}{e}_{z,k}^2$	E1 + E2
	(error squared sum when the foot is on the ground)	(error squared sum when the foot is in the air)
Kalman filter	0.0008	0.0807	0.0815
Proposed smoother	0.0009	0.0020	0.0029

The z-axis estimation error squared sum is given in Table 2. The estimation error of both algorithms during the zero velocity interval (k ∈ Z_m) are almost the same, since the estimated values becomes zero with the flat floor assumption. The estimation error when the foot is in the air are different; the proposed smoother gives significantly smaller estimation error: 0.0020 in the case of the proposed smoother and 0.0807 in the case of the Kalman filter. Furthermore, the maximum z-axis position errors for the Kalman filter and the proposed smoother are 4 cm and 0.5 cm, respectively. This improved estimation is due to the fact the smoother uses all of the available information.

The position estimation by the Kalman filter and the proposed smoother without the flat floor assumption are given in Figure 4. Without the flat floor assumption, we can see that the z-axis error increases since the z-axis position error is not corrected using Equation (14).

Figure 4.

Estimated position without the flat floor assumption: z-axis position (Left) and xy-axis position (Right).

Due to the viewpoint constraint of the optical tracker, the walking distance is rather limited in the first experiment. In the second experiment, a person walked 50 m along a straight line on the flat floor. In this experiment, there is no ground truth value, except the z-axis value, which should be almost constant during the zero velocity intervals, since a person walked on a flat floor.

The Kalman filter estimated and smoother estimated positions are given in Figure 5, both with the flat floor assumption. In the Kalman filter result, the estimated xy trajectory is slightly curved. The smoother estimated xy trajectory is a little bit more straight. The true trajectory should be almost a straight line, since the person walked along a straight line.

Figure 5.

Estimated position (50-m walk): z-axis position (Left) and xy-axis position (Right).

Similar to the first experiment, there is no significant improvement in the smoother position estimation for the position where the foot is on the floor (that is, after the zero velocity updating). The smoother, however, gives more accurate foot motion estimation while the foot is moving in the air, which is important in the gait analysis. This can be seen in the z-axis graphs: the Kalman filter-estimated z-axis trajectory is not accurate, since the z-axis value cannot be negative (which means the foot is beneath the floor).

The 50-m walk is repeated by four subjects: five times for each person, thus a total of 20 experiments. The total walk length estimation is given in Table 3. The average estimated walk length is 48.99 m with the standard deviation being 0.26 m. From the small standard deviation, the estimation result does not seem to depend on the subjects.

Table 3.

Four persons' 50-m walk experiment results.

Subject Number	Final xy Position Estimated Value (m)	Average (m)
1	48.97, 48.97, 49.16, 49.06, 49.02	49.04
2	49.36, 49.29, 49.21, 49.37, 49.14	49.27
3	48.52, 48.98, 48.46, 48.69, 48.45	48.62
4	48.98, 49.00, 48.92, 49.08, 49.06	49.01

We note that the xy-axis position error increases as the walk length increases. This is unavoidable, since the motion is estimated using the inertial navigation algorithm without any external reference. The z-axis position error increment is limited with the flat floor assumption.

The computation time of the proposed smoother was 8.7 s (MATLAB running on Intel Core i5-2310 CPU Windows PC, 2.9 GHz), where the walk time is 58.92 s (N = 5892).

7. Conclusions

In this paper, a smoother algorithm is proposed for gait analysis to obtain more accurate foot motion estimation than the filter-based estimation. In the smoother algorithm, the motion estimation problem is formulated as a quadratic optimization problem, and an efficient computational algorithm is given.

In the first experiment, we have compared the results of the Kalman filter and the proposed smoother. The Kalman filter-based estimation is quite accurate after the zero velocity update. However, foot motion estimation is not accurate when the foot is in the air. We have shown that the motion estimation is significantly improved for the foot motion in the air if the smoother is used: the z-axis position error squared sum when the foot is in the air is 0.0807 (Kalman filter) and 0.0020 (the proposed smoother). Thus, the proposed smoother is a useful algorithm for gait analysis, where the foot motion estimation is important.

In the second experiment, we investigated a long walk foot motion estimation (50 m). There is not much difference in the final position estimation of the Kalman filter and the proposed smoother. The 50-m walk was tested using four subjects, and we found that the estimation does not depend on the subject. This is not surprising, since there are no person-dependent parameters in the algorithm.

Since the proposed smoother algorithm is more complicated than the Kalman filter, the computational time is slow. In the second experiment, the computation time of the proposed smoother is about 8.7 s to process about one minute of walk data (the sampling period is 0.01 s), while it took 0.8 s for the Kalman filter. Thus, the estimation performance is improved with the additional computation time.

A. Appendix

In this Appendix, M₁₁, M₁₂, M₁₃, M₂₁, M₂₂ (partition of M₁ and M₂ in Equation (33)) are defined. We use the notation M₁₁[i, j], which represents a R^9×9 matrix extracting 9(i − 1) + 1 ∼ 9i rows and 9(j − 1) + 1 ∼ 9j columns of M₁₁. Similarly, we use M₁₂[i] ∈ R^9×6 and M₂₁[i] ∈ R^9×1 notations.

• M₁₁

  $$\begin{array}{c}{M}_{11}\left[k,k\right]=\{\begin{array}{ll}{P}_{X_{\mathit{\text{init}}}}^{-1}+{\mathrm{\Phi }}_{21}^{\prime }{Q}_k^{-1}{\mathrm{\Phi }}_{21}\hfill & k=1\hfill \\ {\mathrm{\Phi }}_{k+1,k}^{\prime }{Q}_k^{-1}{\mathrm{\Phi }}_{k+1,k}+Q_k^{-1},\hfill & 2\le k\le N-1\hfill \\ Q_k^{-1},\hfill & k=N.\hfill \end{array}\\ M_{11}\left[k,k\right]=M_{11}\left[k,k\right]+{\tilde{H}}_k^{\prime }{\tilde{R}}^{-1}{\tilde{H}}_k,k\in Z_m\\ \begin{array}{cc}{M}_{11}\left[k,k+1\right]=-{\mathrm{\Phi }}_{k+1,k}^{\prime }{Q}_k^{-1},& 1\le k\le N-1\end{array}\\ \begin{array}{cc}{M}_{11}\left[k+1,k\right]=-Q_k^{-1},{\mathrm{\Phi }}_{k+1,k},& 1\le k\le N-1\end{array}\end{array}$$
• M₁₂

  $$M_{12}\left[k\right]=\{\begin{array}{ll}{\mathrm{\Phi }}_{k+1,k}^{\prime }{Q}_k^{-1}{\mathrm{\Psi }}_{k+1,k},\hfill & k=1\hfill \\ {\mathrm{\Phi }}_{k+1,k}^{\prime }{Q}_k^{-1}{\mathrm{\Psi }}_{k+1,k}-{\mathrm{\Psi }}_{k,k-1},\hfill & 2\le k\le N-1\hfill \\ -{\mathrm{\Psi }}_{k,k-1},\hfill & k=N.\hfill \end{array}$$
• M₁₃

  $$M_{13}=\left(\text{∑}_{k=1}^{N-1}{\mathrm{\Psi }}_{k+1,k}^{\prime }{Q}_k^{-1}{\mathrm{\Psi }}_{k+1,k}\right)+\left[\begin{array}{cc}{P}_{b_{g,\mathit{\text{init}}}}^{-1}& 0_{3\times 3}\\ 0_{3\times 3}& P_{b_{a,\mathit{\text{init}}}}^{-1}\end{array}\right]$$
• M₂₁

  $$M_{21}\left[k\right]=\{\begin{array}{ll}-{\zeta }_k^{\prime }{Q}_k^{-1}{\mathrm{\Phi }}_{k+1,k},\hfill & k=1\hfill \\ -{\zeta }_k^{\prime }{Q}_k^{-1}{\mathrm{\Phi }}_{k+1,k}+{\zeta }_{k-1}^{\prime }{Q}_k^{-1},\hfill & 2\le k\le N-1\hfill \end{array}$$
• M₂₂

  $$M_{22}=\left(\text{∑}_{k=1}^{N-1}-{\zeta }_k^{\prime }{\mathrm{\Psi }}_{k+1,k}\right)+\left[\begin{array}{cc}{\left({\widehat{b}}_{g,SM}-b_{g,\mathit{\text{init}}}\right)}^{\prime }{P}_{b_{g,\mathit{\text{init}}}}^{-1}& 0\\ 0& {\left({\widehat{b}}_{a,SM}-b_{a,\mathit{\text{init}}}\right)}^{\prime }{P}_{b_{a,\mathit{\text{init}}}}^{-1}\end{array}\right]$$

Conflicts of Interest

The author declares no conflict of interest.