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 to rotation angle .
The first step is to calculate the sensor position in the rotor reference frame.
Use a rotation matrix 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 in the CF reference frame and
in the global reference frame. The sensor position
in the base station reference frame is
Finally, the sensor position in the rotor reference frame is
The measurement is the rotation angle when the sensor is hit by the light plane.
To calculate the predicted rotation angle we have to go from the sensor position
( 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
is the sum of the predicted rotation angle and the rotation angle from the
intersection line to the sensor , caused by the tilt of the light plane,
The rotation angle to the sensor is defined by
To calculate we first have to look at the sensor position projected on the XY-plane
. The radius to this point is
We also need the distance from the intersection line to the sensor, perpendicular to the
intersection line, .
can also be calculated using the tilt and z, . If we combine these
Finally we can calculate the predicted rotation angle
Calculate the position elements of the H vector in the rotor reference frame
Which leads to
Rotate the position elements to the global reference frame to be used in the kalman filter
Finally we have the H vector
For rotation matrices the following is true