This page describes the generalized measurement model used for the lighthouse in the Kalman state estimator.
In the measurement model we want to get from a sensor position s to rotation angle α.
The first step is to calculate the sensor position in the rotor reference frame.
Use a rotation matrix Rr to go from the base station reference frame to the rotor reference frame.
For LH2 and the horizontal rotor in LH1 this is the unit matrix, while the vertical drum in LH1 gets
The sensor has position scf in the CF reference frame and
s=pcf+Rcf⋅scf in the global reference frame. The sensor position
in the base station reference frame is sbs=Rbs−1⋅(s−pbs)=Rbs−1⋅(pcf−pbs+Rcf⋅scf)
Finally, the sensor position in the rotor reference frame is sr=Rr⋅Rbs−1⋅(pcf−pbs+Rcf⋅scf)
The measurement is the rotation angle α when the sensor is hit by the light plane.
To calculate the predicted rotation angle αp we have to go from the sensor position
(sr=(x,y,z) in the rotor reference frame) to rotation angle, where the rotation angle is from
the X-axis to the line where the light plane intersects the XY-plane. The rotation angle to the sensor
αs is the sum of the predicted rotation angle αp and the rotation angle from the
intersection line to the sensor αt, caused by the tilt of the light plane,
The rotation angle to the sensor αs is defined by
To calculate αt we first have to look at the sensor position projected on the XY-plane
(x,y,0). The radius to this point is r=x2+y2
We also need the distance d from the intersection line to the sensor, perpendicular to the
intersection line, d=rsinαt.
d can also be calculated using the tilt and z, d=ztan−t. If we combine these