Skip to main content

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

From: Using force data to self-pace an instrumented treadmill and measure self-selected walking speed

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)