## Materials and Methods
### Collision Avoidance Velocity
The goal of the ORCA algorithm is the calculation of the collision-avoiding velocities for agents \(A_{n}\) and \(A_{k}\) within the time \(\tau\). There are additional assumptions for ORCA algorithms. Each of the agents is aware of other agents' parameters: their current position \(p_{n}\), current velocity \(v_{n}\) and radius \(r_{n}\). Each of the agents also has some private parameters: maximum speed \(v_{n}^{max}\) and a preferred velocity \(v_{n}^{pref}\). A preferred velocity is the velocity the agent assumes without any collision with obstacles or other agents. There is an infinite number of pairs of velocities \(v_{n}\) and \(v_{k}\) that are collision-avoiding. ORCA selects the pair that is close to \(v_{n}^{pref}\) and \(v_{k}^{pref}\) velocities of the agents. ORCA finds the smallest change \(u\) required to the relative velocity \((v_{n}-v_{k})\) to avert collision within time \(\tau\). The overall approach is as follows. Agent \(A_{n}\) performs a continuous cycle of sensing and acting with time step \(\Delta t\). In each iteration, the agent acquires the radius, the current position of the agents (and of itself). Based on this information, the \(A_{n}\) infers the permitted plane of velocities \(CA_{A_{n}|A_{k}}^{\tau}\) with respect to \(A_{k}\). In the case of INS error correction, the simulated agent is the agent who should be navigated. We obtain the position for each step of the agent and try to calculate the velocity for inner-steps. The agent needs to be guided along the path defined by radio equipment. For each step performed by the agent, a new position comes to the algorithm. The velocity vector and speed (scalar) are calculated by subtraction of the position coming from radio equipment and the previous position of the agent. For the location of agents inside the building, we create a simulation environment that consists of the following: (a) an agent \(A_{n}\), where \(p_{n}\) is the center of the disc representing the agent and \(r_{n}\) is the radius of the agent \(n\); (b) a set of obstacles; (c) a set of M positions from radio equipment \(\mathcal{O}=\{O_{1},\cdots,O_{m},\cdots,O_{M}\}\). Since the agent in the software should reconstruct the path of the individual in the real environment, the velocity of the individual and the agent must be the same. The average stepwise velocity of the individual is measured, and set as the preferred velocity \(v_{n}^{pref}\) to the agent. The stepwise velocity of the individual is calculated by subtracting the last position \(O_{m}\) and last but one position of the individual coming from radio equipment \(O_{m-1}\) divided by the difference of the timestamps of these positions. \[v_{n}^{pref}=\frac{O_{m}-O_{m-1}}{t_{O_{m}}-t_{O_{m-1}}} \tag{7}\]  
The general idea of error correction is advancing (when necessary) the agent's \(v_{n}^{pref}\), on the basis of positions coming from radio equipment; then, checking whether they collide with an obstacle. If this case holds, we calculate the collision avoidance velocity and correct navigation. The algorithm should be aware of building obstacles. Therefore, at the beginning, the geometry of the environment (where the localisation takes place) should be defined. An open source library from project Aamks ([[http://github.com/aamks](http://github.com/aamks)]([http://github.com/aamks](http://github.com/aamks))) is utilized for this purpose. The library contains a CAD plug-in that allows for drawing building layout in AutoCAD. Then, using the same library, the AutoCAD drawing is transformed into a set of cuboid obstacles. Each of the cuboids is defined by the four-tuple coordinates. The output from the library is a _json_ file as presented below:  
obstacles\": {  
\"1\": [  
[ [ 1394, 994 ], [ 1500, 994 ], [ 1500, 990 ], [ 1394, 990 ] ],  
[ [ 1135, 990 ], [ 1014, 990 ], [ 1014, 994 ], [ 1135, 994 ] ],  
... In this file, \"1\" reflect the floor of the building and contains obstacles at this floor. The json file is then imported to the algorithm, creating the obstacles for the agent.