Location Computation

Similar to the idealistic model described in Section 4.1, the location of the observer can be obtained by determining the intersection point(s) of the three rotational hyperboloids defined by Equation 9. However, since the observed virtual beam width $b$ now additionally depends on the height $h$ of the observer, we have to take into account the exact lighthouse positions. Figure 6, which shows an extended version of Figure 3, illustrates this. The marks on the coordinate axes show the positions of the lighthouse center (as defined in Section 4.2.2) of each of the three lighthouses. That is, the coordinates of the observer are $(x_0+h_x, y_0+h_y, z_0+h_z)$ with respect to the origin formed by the intersection of the three lighthouse rotation axes. In order to obtain approximations for the values $h_x$, $h_y$, and $h_z$, we have to solve the following equation system:

Figure 6: Positions of the lighthouses in the coordinate system. $d_z$ is not shown for clarity.

$$\begin{array}{rcl} 2 d_x \sin \frac{\alpha_x}{2} & = & C_x^b... ...2 \\ d_z^2 & = & (x_0 + h_x)^2 + (y_0 + h_y)^2 \\ \end{array}$$ (10)

The indices $\{x, y, z\}$ indicate which lighthouse the values are associated with. As with equation system 4, this system does not necessarily have a solution, since the parameters are only approximations obtained by measurements. Therefore, minimum mean square error (MMSE) methods have to be used to obtain approximations for the $h_\ast$. However, if the equation system 10 has a solution, we can approximately solve it by simple iteration. For this, we first transform each of the six equations of equation system 10 in order to obtain the following fixpoint form:

$$\begin{array}{rcl} h_x & = & f_1 (d_x) \\ h_y & = & f_2 (d_... ...= & f_5 (h_x, h_z) \\ d_z & = & f_6 (h_x, h_y) \\ \end{array}$$ (11)

Note that we did not show arguments of the $f_i$ (i.e., $C_\ast^\ast$, $\alpha_\ast, x_0, y_0, z_0$) that do not change during iterative evaluation of the equation system. By using appropriate values for $h_x^0, h_y^0, h_z^0$, and $\Delta$, we can obtain approximate solutions for $h_x, h_y, h_z$ with the following algorithm:

		 $h_x := h_x^0;$ 
                 $h_y := h_y^0;$ 
                 $h_z := h_z^0;$ 
                 while (true) { 
                      $h_x' := f_1 (f_4 (h_y, h_z));$ 
                      $h_y' := f_2 (f_5 (h_x, h_z));$ 
                      $h_z' := f_3 (f_6 (h_x, h_y));$ 
                      if ($\vert h_x' - h_x\vert + \vert h_y' - h_y\vert + \vert h_z' - h_z\vert < \Delta$) 
                          break; 
                      $h_x := h_x';$ 
                      $h_y := h_y';$ 
                      $h_z := h_z';$ 
                 }

At first, the $h_\ast$ are initialized to the start values $h_\ast^0$. Using the $f_i$, new approximations $h_\ast'$ are computed. We are finished if the new values are reasonably close to the original $h_\ast$. Otherwise we update the $h_\ast$ to the new values and do another iteration. For good convergence of this algorithm the partial derivatives of the $f_i \circ f_{3+i}$ in the environment of the solution $(h_x,h_y,h_z)$ should be small, which is typically true. In our prototype implementation we use $h_\ast^0 := 100$cm and $\Delta := 0.1$cm. With this configuration, the algorithm typically performs 4-6 iterations.