Table 1 Pseudo code of Kalman filter for walking speed and position estimation

0: p_KF=0, v_KF=0, P=P₀

initialize

1: loop

(every time step)

2: \(\left [\begin {array}{ll} p_{KF}\\ v_{KF} \end {array}\right ] = A \left [\begin {array}{ll} p_{KF}\\ v_{KF} \end {array}\right ] + B \left [\begin {array}{ll} a_{mes}\end {array}\right ]\)

time update: predict speed and position

3: P=A·P·A^T+B·Q·B^T

update error covariance matrix

4: if a new foot contact is detected

(every footstep)

5: K=P·(P+R)⁻¹

update Kalman gain

6: \(\left [\begin {array}{ll} p_{KF}\\ v_{KF} \end {array}\right ] = \left [\begin {array}{ll} p_{KF}\\ v_{KF} \end {array}\right ] + K \left (\left [\begin {array}{ll} \bar {p}_{mes}\\ \bar {v}_{mes} \end {array}\right ] - \left [\begin {array}{ll} \bar {p}_{KF} \\ \bar {v}_{KF} \end {array}\right ] \right)\)

measurement update: correct speed and position

7: P=(I−K)·P

update error covariance matrix

8: end if

9: end loop

\(\hspace {-5pt}A=\left [\begin {array}{ll} 1 & \Delta t\\ 0 & 1 \end {array}\right ]\), \(B=\left [\begin {array}{ll} \frac {\Delta t^{2}}{2}\\ \Delta t \end {array}\right ]\), Δt=0.001, \(Q= B\cdot B^{T}\cdot \sigma ^{2}_{a}=2.9\times \left [\begin {array}{ll} \frac {\Delta t^{4}}{4} & \frac {\Delta t^{3}}{2}\\ \frac {\Delta t^{3}}{2} & \Delta t^{2} \end {array}\right ]\)

\(\hspace {-8pt}R= \left [\begin {array}{ll} \sigma _{p}\\ \sigma _{v} \end {array}\right ] \cdot \left [\begin {array}{ll} \sigma _{p}\\ \sigma _{v} \end {array}\right ]^{T}=10^{-3} \times \left [\begin {array}{ll} 0.6 & 0\\ 0 & 7.2 \end {array}\right ]\), \(P_{0}=10^{-3} \times \left [\begin {array}{ll} 3.5 & 1.5\\ 1.5 & 1.6 \end {array}\right ]\)