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

Pseudo code Note
0: pKF=0, vKF=0, P=P0 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·AT+B·Q·BT update error covariance matrix
4: if a new foot contact is detected (every footstep)
5: K=P·(P+R)−1 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=(IKP 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 ]$$
1. Note that we omitted the observation matrix in lines 5 7 as it is the identity matrix (C=I)