## 4 Photogrammetric Reconstruction with the UWB Positioning System
\right]+w_{t} \tag{1}\]  
where \(x_{t}\) is the rover 2D position at time \(t\) on the planar surface at constant altitude, \(T\) is the length of the time interval between the two estimates and \(w_{t}\) is the model noise. The measurement equation for anchor \(i\) is:  
\[y_{t,i}=\sqrt{(x_{t,u}-q_{i,u})^{2}+(x_{t,v}-q_{i,v})^{2}+h^{2}}+z_{t} \tag{2}\]  
where \(y_{t,i}\) is the measurement of anchor \(i\) at time \(t\), \(q_{i}\) is the position of anchor \(i\) (\(u\) and \(v\) are two orthogonal direction on the horizontal plane) and \(z_{t}\) is the measurement noise. Future investigation foresees also the integration with pedestrian positioning estimation methods [47; 48; 49]. The computed rover positions in the \(\Sigma_{uvb}\) metric coordinate system, are used to estimate the parameters (translation vector, rotation matrix, scale factor) for mapping the photogrammetric point clouds from the photo to UWB frame. Least squares fitting (minimizing the sum of squared residuals) provided the best values for the parameters. In the scaling process, the same weights have been given to all camera positions estimated by the UWB system. However, positions far from UWB anchors are clearly affected by a higher estimation error with respect to those surrounded by anchors. Introducing different weights depending on the considered position might improve scale estimation performance and will be the object of our future investigation. Similarly to [8], the optimization problem considered for scale estimation was also extended to solve the synchronization between camera and UWB sensors measurements. A further improvement of the above estimates can be obtained by taking into account also misalignments due to the lever arm and boresight between the camera and rover. The rover was fixed with a proper position and orientation in order to make the axes of its coordinate system have approximately the same orientation of those of the camera, and translation between their centers is mostly in a single direction (the vertical axis). Calibration of the camera-UWB rover system was performed similarly to the procedure described in [50]. The correction of camera positions can be applied once an estimate of the transformation from \(\Sigma_{photo}\) to \(\Sigma_{uwb}\) is available. Then, the transformation \(\Sigma_{photo}\) to \(\Sigma_{uwb}\) is re-estimated with the updated coordinates, and the process can be iterated until convergence. Georeferencing of the photogrammetric model is obtained as follows: first, the UWB rover is set outside of the bastion over the position of the GNSS CP closest to the tunnel entry. This allows estimating the translation term between the \(\Sigma_{uwb}\) local frame and the global mapping frame (WGS84, UTM32). North and vertical directions are estimated by exploiting IMU measurements (the IMU was pre-calibrated as described in [40], i.e., calibration of accelerometer and magnetometer): magnetometer and accelerometer data, collected in the rover reference frame are first transformed in the coordinate systems of the cameras and then in the \(\Sigma_{uwb}\) system by using camera orientations (computed during the photogrammetric reconstruction) and the previously-estimated transformation from \(\Sigma_{photo}\) to \(\Sigma_{uwb}\). To be more specific, let \(g_{rover,i}\) and \(m_{rover,i}\) be the (calibrated) accelerometer and magnetometer measurement vectors taken during the acquisition of the \(i\)-th image. Since during image acquisition the camera is assumed to be still, accelerometer measurement \(g_{rover,i}\) corresponds to a measurement of the vertical direction in the rover reference frame (i.e., the opposite of the gravity direction). Furthermore, since they correspond to directions, they can be assumed to be unit vectors (i.e., they can be normalized).