Lighthouse

The Lighthouse positioning system allows for a low cost alternative to many other motion capture systems. By sweeping the area with infrared and measuring the impact angles on the car mounted deck, position and orientation of the car can be computed.

Measurement Function

The measurement model can be obtained as

\[ y_{\mathrm{lh}} = \begin{bmatrix} \alpha_{1,1} &\alpha_{2,1} & \alpha_{3,1} & \alpha_{4,1} & \alpha_{1,2} & \alpha_{2,2} & \alpha_{3,2} & \alpha_{4,2} \end{bmatrix} \]

where \(\alpha_{k,l}\) is the impact angle for sensor \(k\) and light plane \(l\). The impact angles are given as

\[ \begin{aligned} \alpha_{k,l} = \tan^{-1}\left(\frac{y_{\mathrm{lh},k}}{x_{\mathrm{lh},k}}\right) + \sin^{-1}\left(\frac{z_{\mathrm{lh},k} \tan(t_{l})}{ \sqrt{x_{\mathrm{lh},k}^2 + y_{\mathrm{lh},k}^2}}\right) \end{aligned} \]

where \(t_{1} = - \frac{\pi}{6} - \delta t_{1}\) and \(t_{2} = \frac{\pi}{6} - \delta t_{2}\) are the tilt angles of the two light planes of each base station, and \(\delta t_{1}\) and \(\delta t_{2}\) are factory calibrated offsets.

The position of each sensor \({}_{\mathcal L}p_{\mathrm{lh},k} = \begin{bmatrix} x_{\mathrm{lh},k} & y_{\mathrm{lh},k} & z_{\mathrm{lh},k} \end{bmatrix}^\top\), in the Lighthouse base station frame, is related to the position of the car via the transformation

\[ \begin{aligned} {}_{\mathcal L}p_{\mathrm{lh},k} = R_{\mathrm{bs}}^\top \left(\begin{bmatrix} \cos (\psi) & -\sin (\psi) \\ \sin (\psi) & \cos (\psi) \\ 0 & 0 \end{bmatrix} {}_{\mathcal{B}}p_{\mathrm{lh,k}}+ \begin{bmatrix} x_{\mathrm{p}} \\ y_{\mathrm{p}} \\ 0 \end{bmatrix} - p_{\mathrm{bs}} \right) \end{aligned} \]

where \({}_{\mathcal{B}}p_{\mathrm{lh,k}}\) is the position of the \(k\)-th sensor in body frame and \(R_{\mathrm{bs}}, p_{\mathrm{bs}}\) are the rotation and position of the Lighthouse base station.