## Materials and Methods
### Collision Avoidance Velocity
The next step is to import of data from electronic devices. The data coming from INS constitute the following frame: _timestamps_ of the step, \(x,y,z\) position and the _distance_ of the agent from an anchor (the anchor is defined in Section 3.1). timestamp, x, y, z, distance
750286, -0.001, -0.001, -0.000, {1: 0.78}
756754, 3.173, 0.372, 0.198, {1: 4.46}
757815, 4.512, 0.648, 0.254, {1: 5.77}
758939, 5.950, 0.792, 0.340, {1: 7.12}
759945, 6.563, 0.851, 0.343, {1: 7.69}  
In the navigation process, we address agent position on 2D space. The \(z\) coordinate is excluded from error correction, and is used as an elevation parameter. There is no collision detection with ceilings or floors. The positions returned from radio equipment are calculated as a relative distance from the starting point of the agent (start of the navigation). The starting position is always (0, 0, 0). Let us call this system a _local coordinate system_ (LCS). The first step in INS is always advancement in all coordinates. Hence, LCS can be rotated, depending on the direction in which the individual starts moving. There is also _a global coordinate system_ (GCS), the system where the building is situated, defined for example by the GPS position. In order to visualize the agent on the building layout, it is necessary to translate and rotate LCS to adjust GCS. We assume that this operation will be performed when INS is working and the data from GNSS are still available (at the entrance to the building). The translation and rotation operation is stored in _an update matrix_ defined as follows:  
\[ \begin{bmatrix}cos\theta&-sin\theta&d_{x}\\ sin\theta&cos\theta&d_{y}\\ 0&0&1\end{bmatrix}  \tag{8}\]  
where \(d_{x},d_{y}\) is a translation vector and \(\theta\) is the angle of rotation. Hence, the adjustment of LCS to GCS will be based on the following calculations:  
\[ \begin{bmatrix}x_{g}\\ y_{g}\end{bmatrix} = \begin{bmatrix}cos\theta&-sin\theta&d_{x}\\ sin\theta&cos\theta&d_{y}\\ 0&0&1\end{bmatrix} \cdot \begin{bmatrix}x_{l}\\ y_{l}\\ 1\end{bmatrix}  \tag{9}\]  
where \(x_{g},y_{g}\) are coordinates in GCS and \(x_{l},y_{l}\) are coordinates in LCS. The transformed points from LCS are then used to calculate the average stepwise velocity of the individual, and then set to the agent. Then, the agent moves to the transformed position coming from INS. The operation is repeated for each of the consecutive positions coming from INS. Figure 3 depicts the positions coming from radio INS. Between each consecutive pair of points, we calculate the velocity of the individual and place it in the simulation environment. The operation is repeated during the entire navigation process, guiding the agent through the building. When the calculated velocity (on the basis of the current and next position of the individual) collides with a wall, ORCA corrects the velocity vector. The proposed position avoids the obstacles and is defied by a new velocity with collision avoidance. Then, the angle between INS-defined velocity and collision avoidance velocity is calculated. The angle constitutes an update for navigation. Figure 4 shows how the angle of update is calculated. The angle of update \(\theta_{i}\) needed to avoid collision with an obstacle is then used to change the value in the defined _update matrix_. Then, each of the following positions is updated according to these corrections. The final algorithm for error correction is presented in pseudocode Algorithm 1. Additional assumptions are required as not all the collisions were caused by INS error in rotation. Some of them were caused by error in the distance calculation, eventually missing the proper doors. In order to discover that the error is due to a bad reflection in rotation, we assume that this type of error generates a rather small update in angle \(\theta_{i}\). This means that if \(\theta_{i}\) is, for example, \(90^{\circ}\), this is not an error in rotation, but rather some other problem. This means that an agent most likely crossed a door; however, due to the error in location, we missed this door.