Lighthouse angle conversion

One way to get started with lighthouse 2 is to create a conversion from lighthouse 2 angles to lighthouse 1 angles, and use the functionality that has already been implemented for lighthouse 1. Even though it is an easy way to get started, the main drawback is that we need both sweeps for the conversion. Lighthouse 1 angles are also easier to understand and can be a useful debugging tool.

Lighthouse 1 to lighthouse 2

Start from the measurement model for the kalman filter.

For a point (x,y,z)(x, y, z), the sweep angles are

{α1lh2=tan1(yx)+sin1(ztan(t)x2+y2)α2lh2=tan1(yx)+sin1(ztan(t)x2+y2)\begin{cases} \alpha^{lh2}_1 = \tan^{-1}(\frac{y}{x}) + \sin^{-1}(\frac{z\tan(-t)}{\sqrt{x^2 + y^2}})\\ \alpha^{lh2}_2 = \tan^{-1}(\frac{y}{x}) + \sin^{-1}(\frac{z\tan(t)}{\sqrt{x^2 + y^2}}) \end{cases}

where t=π6t = \frac{\pi}{6}

xx, yy and zz are given by the lighthouse 1 sweep angles

{x=1y=tan(α1lh1)z=tan(α2lh1)\begin{cases} x = 1 \\ y = \tan(\alpha^{lh1}_1) \\ z = \tan(\alpha^{lh1}_2) \end{cases}

this leads to

{α1lh2=α1lh1+sin1(tan(α2lh1)tan(t)1+tan2(α1lh1))α2lh2=α1lh1+sin1(tan(α2lh1)tan(t)1+tan2(α1lh1))\begin{cases} \alpha^{lh2}_1 = \alpha^{lh1}_1 + \sin^{-1}(\frac{\tan(\alpha^{lh1}_2)\tan(-t)}{\sqrt{1 + \tan^2(\alpha^{lh1}_1)}})\\ \alpha^{lh2}_2 = \alpha^{lh1}_1 + \sin^{-1}(\frac{\tan(\alpha^{lh1}_2)\tan(t)}{\sqrt{1 + \tan^2(\alpha^{lh1}_1)}})\\ \end{cases}


{α1lh2=α1lh1+sin1(qtan(t))α2lh2=α1lh1+sin1(qtan(t))\begin{cases} \alpha^{lh2}_1 = \alpha^{lh1}_1 + \sin^{-1}(q\tan(-t))\\ \alpha^{lh2}_2 = \alpha^{lh1}_1 + \sin^{-1}(q\tan(t))\\ \end{cases}


q=tan(α2lh1)1+tan2(α1lh1)q = \frac{\tan(\alpha^{lh1}_2)}{\sqrt{1 + \tan^2(\alpha^{lh1}_1)}}

Lighthouse 2 to lighthouse 1

The strategy when creating an equation to convert from lighthouse 2 sweep angles to lighthouse 1, is to find the intersection line between the two light planes. From the intersection line we can easily calculate the lighthouse 1 sweep angles.

The following calculations are all in the base station reference frame.

Assume we know the normals for the two light planes, n1\vec{n_1} and n2\vec{n_2}. The cross product of the normals v=n1×n2\vec{v} = \vec{n_1} \times \vec{n_2} gives us the direction of the intersection line. We also know it is passing through the origin as we are using the base station reference frame.

Normals of the light planes

Start by figuring out the normals when α=0\alpha=0, and then rotate them around the Z-axis using a rotation matrix.

The rotation matrix is

Rz=[cosαsinα0sinαcosα0001]R_{z} = \left[\begin{array}{ccc} cos{\alpha} & -sin{\alpha} & 0 \\ sin{\alpha} & cos{\alpha} & 0 \\ 0 & 0 & 1 \end{array}\right]

and the resulting normals

{n1=Rz[0cos(t)sin(t)]=[cos(t)sin(α1lh2)cos(t)cos(α1lh2)sin(t)]n2=Rz[0cos(t)sin(t)]=[cos(t)sin(α2lh2)cos(t)cos(α2lh2)sin(t)]\begin{cases} \vec{n1}=R_z \cdot \begin{bmatrix}0 & -cos{(t)} & sin{(t)}\end{bmatrix} = \begin{bmatrix}cos{(t)}sin{(\alpha^{lh2}_1)} & -cos{(t)}cos{(\alpha^{lh2}_1)} & sin{(t)}\end{bmatrix} \\ \vec{n2}=R_z \cdot \begin{bmatrix}0 & -cos{(t)} & -sin{(t)}\end{bmatrix} = \begin{bmatrix}cos{(t)}sin{(\alpha^{lh2}_2)} & -cos{(t)}cos{(\alpha^{lh2}_2)} & -sin{(t)}\end{bmatrix} \end{cases}

where t=π/6t=\pi/6 is the tilt angle of the light planes.

The intersection vector

v=n1×n2=[sin(t)cos(t)(cos(α1lh2)+cos(α2lh2))sin(t)cos(t)(sin(α1lh2)+sin(α2lh2))cos2(t)(sin(α1lh2)cos(α2lh2)cos(α1lh2)sin(α2lh2))]\vec{v} = \vec{n_1} \times \vec{n_2} = \begin{bmatrix}-\sin{(t)}\cos{(t)}(\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)}) & -\sin{(t)}\cos{(t)}(\sin{(\alpha^{lh2}_1)} + \sin{(\alpha^{lh2}_2)}) & \cos^2{(t)}(\sin{(\alpha^{lh2}_1)}\cos{(\alpha^{lh2}_2)}-\cos{(\alpha^{lh2}_1)}\sin{(\alpha^{lh2}_2)})\end{bmatrix}

Lighthouse 1 angles

Finally we can calculate the lighthouse 1 angles

{α1lh1=tan1(v2v1)α2lh1=tan1(v3v1)\begin{cases} \alpha^{lh1}_1 = \tan^{-1}(\frac{v_2}{v_1})\\ \alpha^{lh1}_2 = \tan^{-1}(\frac{v_3}{v_1}) \end{cases} {α1lh1=tan1(sin(t)cos(t)(sin(α1lh2)+sin(α2lh2))sin(t)cos(t)(cos(α1lh2)+cos(α2lh2)))α2lh1=tan1(cos2(t)(sin(α1lh2)cos(α2lh2)cos(α1lh2)sin(α2lh2))sin(t)cos(t)(cos(α1lh2)+cos(α2lh2)))\begin{cases} \alpha^{lh1}_1 = \tan^{-1}(\frac{-\sin{(t)}\cos{(t)}(\sin{(\alpha^{lh2}_1)} + \sin{(\alpha^{lh2}_2)})}{-\sin{(t)}\cos{(t)}(\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)})})\\ \alpha^{lh1}_2 = \tan^{-1}(\frac{\cos^2{(t)}(\sin{(\alpha^{lh2}_1)}\cos{(\alpha^{lh2}_2)}-\cos{(\alpha^{lh2}_1)}\sin{(\alpha^{lh2}_2)})}{-\sin{(t)}\cos{(t)}(\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)})}) \end{cases} {α1lh1=α1lh2+α2lh22α2lh1=tan1(sin(α2lh2α1lh2)tan(t)(cos(α1lh2)+cos(α2lh2)))\begin{cases} \alpha^{lh1}_1 = \frac{\alpha^{lh2}_1 + \alpha^{lh2}_2}{2}\\ \alpha^{lh1}_2 = \tan^{-1}(\frac{\sin{(\alpha^{lh2}_2 - \alpha^{lh2}_1)}}{\tan{(t)} (\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)})})\end{cases}