## Materials and Methods
### Collision Avoidance Velocity
Therefore, we assumed a range of accepted angles that may be caused by the error in rotations. On the basis of the experiments, we decided that the maximal error for one step caused by the inertial system is no greater than \(30^{\circ}\). Hence, if the algorithm calculates that the correction in velocity--needed to avoid a wall--is less than \(30^{\circ}\), it places the update in the _update matrix_. The final tuning of the threshold value can be defined on the basis of genetic algorithms. This is a goal for future work. Figure 4: A correction of the position—the agent collides with a wall. Ellipsis reflects the agent; purple lines define obstacles; the red arrow defines the velocity vector (as a subtraction of the position from INS and the agent’s current position); the gray arrow defines the collision avoidance velocity; the green line is the original path sent by the radio device. \(P(x_{n},y_{n})\) is the current position of the agent; \(P(x_{n+1},y_{n+1})\) is the next position obtained from INS; \(\theta_{i}\) is an angle for velocity rotation in order to avert collision; \(P(\xi_{n+1},y_{n+1})\) is the corrected position. Figure 3: Positions obtained from radio INS. Between each of the consecutive pairs of points, the velocity of the agent is calculated, stating the input for the ORCA algorithm. The green dots represent the points where the positions were measured. The purple lines represent building obstacles. For the sake of performance, we are also going to avoid having an excessively long _update matrix_. Therefore, we also defined experimentally a lower boundary for the update error. We decided to place the update in the matrix if it is greater than \(2^{\circ}\). The other assumption related to the velocity angle value that needed to be corrected is that, if this correction is around \(90^{\circ}\pm 15^{\circ}\) we suspect that the agent transits compartments; however, we miss the proper door. In this case, we should move the agent position to a door. This is performed by updating the values of the transformation vector \((d_{x},d_{y})\) in the _update matrix_. The vector is defined on the basis of the current agent's position and the door center position defined in the building layout. In many cases, due to error in the location, it is not obvious which door should be chosen. It appeared that choosing the closest door is misleading--from the perspective of the current location of the agent. In an office layout, where a number of doors are placed very close to each other, the choice of the closest door was a source of poor navigation. To address this problem, we used second information sent by INS--_distance_ of the agent from an anchor. We assume that when an agent enters a building, they place the anchor at the entrance. The anchor is used to calculate the point-to-point distance between the receiver worn by an agent, and the anchor. The door selection problem was addressed by minimizing the error in distance with respect to the agent and the anchor. The last assumption is related to the length of the velocity vector calculated on the basis of the individual's current and next position. When the individual enters the room and we miss the door in our application, then they will be kept outside the room (because of wall constraints). The distance between the current agent position (corrected) and that coming from INS will grow. The velocity vector--defined at these distance and timestamps--will also grow, achieving values that are unnatural for humans. This means, for us, that the agent is located in the wrong compartment. In this case, we move the agent to the position proposed by the INS system. The necessary correction is placed in the _update matrix_. Implementing these assumptions in the algorithm improves the correction of the position. Figure 5 depicts the estimated path of the agent without our map-matching algorithm. In comparison, Figure 6 depicts the path after its correction by the algorithm. Figure 5: Estimation of an agent’s path without any map matching algorithm.