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 , the sweep angles are
, and are given by the lighthouse 1 sweep angles
this leads to
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, and . The cross product of the normals
gives us the direction of the intersection line. We also know it is passing through the origin as we are using the base station
Normals of the light planes
Start by figuring out the normals when , and then rotate them around the Z-axis using a rotation matrix.
The rotation matrix is
and the resulting normals
where is the tilt angle of the light planes.
The intersection vector
Lighthouse 1 angles
Finally we can calculate the lighthouse 1 angles