Functionality to handle base station geometry in the lighthouse poistioning system
Classes
LighthouseBsGeoEstimator
LighthouseBsGeoEstimator()
This class is used to estimate the geometry (position and attitude) of a lighthouse base station, given angles measured using a lighthouse deck.
Methods
def estimate_geometry(self, bs_vectors)
Estimate the full pose of a base station based on angles from the 4 sensors on a lighthouse deck. The result is a rotation matrix and position of the base station, in the Crazyflie reference frame.
:param bs_vectors A list of 4 LighthouseBsVector objects specifying vectors to the 4 sensors :return rot_bs_in_cf_coord: Rotation matrix of the BS in the CFs coordinate system :return pos_bs_in_cf_coord: Position vector of the BS in the CFs coordinate system
def is_available(self)
def sanity_check_result(self, pos_bs_in_cf_coord)
Checks if the estimated geometry is within reasonable bounds. It returns true if it seems reasonable or false if it doesn’t