## 3 A Prototype System for Pedestrian Positioning
### 3D Matching Algorithm  
Estimated location based on the stepmeter, gyroscope and digital maps is accurate to within 15 m (a street with pavements on both sides) [3]. GPS errors in urban canyons being a several dozens of metres, practically both sides of a road are equally probable. Figure 12: (**a**) TV camera image. (**b**) Disparity map from the stereo camera. Darker colours correspond to smaller disparity, thus bigger distance. (**c**) 3D model of the corresponding urban environment. Figures 12(a-c) show a picture from a monocamera, a disparity map \(\mathbf{K}\) and a part of the 3D model of the University Campus. Let us use the local coordinate system defined in Section 3.1.2. Let the vector \([x_{S}^{\prime},y_{S}^{\prime},z_{S}^{\prime},\phi_{S}^{\prime}]^{T}\) denote the Cartesian coordinates and the orientation of the stereo camera in the local coordinate system. \(\phi_{S}^{\prime}\) is called the azimuth of the stereo camera and it has the same interpretation as with a magnetic compass. It is assumed that the stereovision camera is worn by a pedestrian and it is attached to the chest, say at \(z=h=1.5\) m above the ground. The optical axis of the camera is directed roughly parallel to the ground plane. The positioning algorithm has a precise 3D model of the environment that is defined for the local coordinate system--see Figure 12(c). Let us put a virtual camera in the \((x_{V}^{\prime},y_{V}^{\prime},z_{V}^{\prime})\) coordinates and set its orientation to \(\phi_{V}^{\prime}\). Then, let us render the virtual environment based on the 3D model. The virtual camera should see the picture presented in Figure 12(c). On the other hand, the stereovision camera provides a depth image of the environment. Both images can be compared. Let \(\mathbf{L_{S}}\) and \(\mathbf{L_{V}}\) denote the depth image returned by the stereovision camera and retrieved from the virtual environment, correspondingly. \(f_{L}(\mathbf{L_{S}},\mathbf{L_{V}})\) is some cost function comparing the two depth images and returning comparison error. The function is defined by Equation (38). \[f_{L}:(\mathbf{L},\mathbf{L})\rightarrow\mathbb{R}^{1} \tag{38}\]  
Formally, the problem comes down to finding the vector \([x_{V}^{\prime},y_{V}^{\prime},z_{V}^{\prime},\phi_{V}^{\prime}]^{T}\) where the function \(f_{L}(\mathbf{L_{S}},\mathbf{L_{V}})\) returns a minimum value, that is, the difference \(||\mathbf{L_{S}}-\mathbf{L_{V}}||\) is minimal according to some criterion. Then the estimated coordinates of the stereo camera (that is the pedestrian location) are given by Equation (39). This task is handled by the particle filtering algorithm. \[[x_{S}^{\prime},y_{S}^{\prime},z_{S}^{\prime},\phi_{S}]^{T}=[x_{V}^{\prime},y_ {V}^{\prime},z_{V}^{\prime},\phi_{V}^{\prime}]^{T} \tag{39}\]  
Assuming \(z_{S}=z_{V}=h\) reduces the dimension of the problem. The function \(f_{L}\) is still not trivial. The depth picture \(\mathbf{L_{S}}\) is encumbered with many errors, see Figure 12(b). For example, sun illumination causes some parts of the depth map to have falsely determined values (see Figure 13). The function \(f_{L}\) should be immune to these undesirable effects that can cause random bias errors, most troublesome errors to correct by any filter, including a particle filter which is used in the presented project. A random bias error is understood as an error whose expected value differs considerably from 0 for a significant period of time, e.g., 30 seconds. There can be also objects that can compromise the comparison between \(\mathbf{L_{S}}\) and \(\mathbf{L_{V}}\) and cannot be filtered by any methods, _i.e._, parked cars, trucks, bushes, posters, garbage bins, _etc._ Their influence can be minimized to some extent by excluding from comparison that part of \(\mathbf{L_{S}}\) which is below the optical axis of the camera. Hence the ground surface is also excluded from comparison. First \(\mathbf{L_{S}}\) is analysed to seek fragments for comparison.