Cross-modality sensor fusion and error correction

A system, comprising:

• a first sensor coupled to a body, the first sensor having a first modality;
• a second sensor coupled to the body, the second sensor having a second modality, the second modality being different from the first modality;
• a third sensor coupled to the body, the third sensor having a third modality, the third modality being different from the first modality and the second modality;
• non-transitory computer-readable storage media storing instructions;
• and
• an electronic processor configured to execute the instructions to:
• initialize a state model, the state model including (i) linear components representing an instantaneous rate of rotation of the body and (ii) quaternion components representing a current orientation of the body,
• apply a linear state estimator to the linear components of the state model based on first sensor signals from the first sensor to determine an estimated angular velocity of the body,
• apply a kinematic model to the quaternion components of the state model to determine an estimated quaternion, the estimated quaternion representing an orientation of the body in a body frame,
• compute a transformation matrix based on the estimated quaternion, the transformation matrix being usable to transform representations from the body frame to a navigation frame,
• determine a first modality-specific measurement based on second sensor signals from the second sensor,
• determine a second modality-specific measurement based on third sensor signals from the third sensor,
• compute a cross-product of the first modality-specific measurement and the second modality-specific measurement,
• transform the cross-product of the first modality-specific measurement and the second modality-specific measurement from the body frame into the navigation frame using the transformation matrix,
• compute a reference cross-product of a first reference measurement and a second reference measurement, the first reference measurement and the second reference measurement being in the navigation frame,
• compute an error between the transformed cross-product and the reference cross-product, and
• correct the estimated quaternion to minimize the error, the corrected quaternion representing a corrected orientation of the body.

A method, comprising:

• initializing, at a sensor platform, a state model, the state model including (i) linear components representing an instantaneous rate of rotation of the sensor platform and (ii) quaternion components representing a current orientation of the sensor platform,
• applying, at the sensor platform, a linear state estimator to the linear components of the state model based on first sensor signals from a first sensor to determine an estimated angular velocity of the sensor platform, the first sensor being coupled to the sensor platform, the first sensor having a first modality;
• applying, at the sensor platform, a kinematic model to the quaternion components of the state model to determine an estimated quaternion, the estimated quaternion representing an orientation of the sensor platform in a body frame;
• computing, at the sensor platform, a transformation matrix based on the estimated quaternion, the transformation matrix being usable to transform representations from the body frame to a navigation frame;
• computing, at the sensor platform, a modality-specific measurement based on second sensor signals from a second sensor, the second sensor being coupled to the sensor platform, the second sensor having a second modality different from the first sensor;
• transforming, at the sensor platform, the modality-specific measurement from the body frame into the navigation frame using the transformation matrix;
• computing, at the sensor platform, an error between the transformed modality-specific measurement and a reference measurement, the reference measurement being in the navigation frame;
• and
• correcting, at the sensor platform, the estimated quaternion to minimize the error, the corrected quaternion representing a corrected orientation of the sensor platform.