## Materials and Methods
### Foot-Mounted Inertial Navigation System  
Foot-Mounted INS is implemented by means of the Kalman Filter by estimating an error state vector \(\delta x_{k}=[\delta\upsilon_{k},\delta p_{k},\delta C_{k}]\), which represents errors in velocity, position and orientation matrix, respectively, at time \(k\). The input vector is formed from sensor readings \(u_{k}=[a_{k},\omega_{k}]\), which represent acceleration \(a_{k}=[a_{x}^{k},a_{y}^{k},a_{z}^{k}]\) and angular rates \(\omega_{k}=[\omega_{x}^{k},\omega_{y}^{k},\omega_{z}^{k}]\) both in the sensor frame of reference. Note that we operate on quaternions in navigation equations, which are later converted to the orientation matrix in error state computations. Therefore, \(u_{k}\) does not match the transition state matrix but is converted by the control-input model described in detail in pseudocode.  
Details of the algorithm are described by pseudocode Algorithm 1. Generally, we maintain a quaternion \(q_{k}\) that rotates from the sensor frame of reference to the navigational frame estimated by some orientation filter (e.g., [35]) using \(\omega_{k}\) (line no. 7). Then, we compute acceleration in the navigational frame (line no. 8) to apply mechanization equations (lines 9-10). In line 17, the error is computed using Kalman Gain \(K\) using zero velocity pseudo-observation (note the observation matrix \(H\)). This is pseudo-observation because we do not measure it directly but use some statistical hypothesis to test whether the feet are still in function IsZUPT (for possible implementations, see [36]. Briefly speaking, one can check whether raw readings from a gyroscope or accelerometer are below a certain threshold).  
State transition matrix \(F_{k}\) is defined as follows:  
\[F_{k}:=\begin{bmatrix}I_{3\times 3}&\Delta tI_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&I_{3\times 3}&0_{3\times 3}\\ \Delta tS_{k}&0_{3\times 3}&I_{3\times 3}\end{bmatrix}, \tag{1}\]  
where \(S_{k}\) is a skew-symmetric cross-product operator matrix given by:  
\[S_{k}:=\begin{bmatrix}0&-a_{z,k}^{N}&a_{y,k}^{N}\\ a_{z,k}^{N}&0&-a_{x,k}^{N}\\ -a_{y,k}^{N}&a_{x,k}^{N}&0\end{bmatrix}. \tag{2}\]  
Figure 1: An example of how small error in rotation causes large error in location. The green line reflects the path received from INS. The purple lines define the walls. The proper path should start and finish at the same point. The estimated path ends with 12 m of error. The axes are defined in meters [m].  
```
1:procedureINS-MM(\(a,\omega,g_{0},A\))
2:\(k\gets 0\)
3:\(q_{0}\leftarrow\text{InitializeOrientation}(g_{0})\)
4:\(p_{l},k_{l}\gets 0,0\)
5:loop
6:\(k\gets k+1\)
7:\(q_{k}\leftarrow\text{UpdateOrientation}(a_{k},\omega_{k})\)
8:\(a_{nav}\gets q_{k}^{-1}\otimes a_{k}\otimes q_{k}-g_{0}\)
9:\(v_{k}^{(+)}\gets v_{k-1}+a_{nav}\Delta t\)
10:\(p_{k}^{(+)}\gets p_{k-1}+v_{k-1}\Delta t\)
11:\(x_{p}^{(+)}\leftarrow[v_{k}^{(+)}\,p_{k}^{(+)}\text{QuatToRotMtx}(q_{k})\,]\)
12:\(P_{k}^{(+)}=FP_{k-1}F^{T}+Q\)
13:ifIsZUPT(\(\omega,k\))then
14:\(H\leftarrow[0_{3\times 3}\,I_{3\times 3}\,0_{3\times 3}]\)
15:\(K\gets P^{(+)}H^{T}(HP^{(+)}H^{T}+R)^{-1}\)
16:\(P_{k}\gets I-KHP_{k}^{(+)}\)
17:\([\delta v_{k}\,\delta p_{k}\,\delta C_{k}]\gets K(0_{3\times 1}-Hx_{k})\)
18:\(q_{k}\leftarrow\text{CorrectOrientation}(C_{k})\)
19:\([p_{k},v_{k}]\leftarrow[p_{k},v_{k}]-[\delta p_{k},\delta v_{k}]\)
20:\(v_{a}\leftarrow(p_{k}-p_{l})\,k\,\Delta t\)
21:\(p_{l},k_{l}\gets p_{k},k\)
22:ifIsCollision(\(p_{k},\mathcal{A}\))then
23:\(\theta_{k}\gets ORCA(v_{a},\mathcal{A})\)
24:\(q_{k}\gets q_{k}\,q_{\theta_{k}}\)
25:endif
26:endif
27:endloop
28:endprocedure
```  
**Algorithm 1** Pseudocode for the inertial positioning system with zero-velocity updates and collision avoidance velocity map matching.