I have a Gimbal (a movable platform) which can move in 3 axis (roll, pitch and heading). I have a Device which is connected directly to the Gimbal which reads back current roll, pitch and heading values.
The Gimbal is within another moving object, the Gimbal must compensate for vehicle movement by reading the Device values back and correcting by moving the platform accordingly.
So if the Device reads Roll = 0, Pitch = 1, Yaw = 0 then we can correct this by moving the Pitch of the Gimbal by -1.
The issue is that as the Gimbal moves to more extremes, the changes to the Gimbal value are no longer direct. For instance if the Gimbal is already at a Roll of +90 degrees, (because of early compensations to keep the Device at 0 degrees) and the Device reports a Pitch = 1, the corrective action of the Gimbal is to move +1 Yaw.
My solution, which does not appear to work is as follows:
Create a Direction Cosign Matrix from the current Euler Angles of the Gimbal. Create a Direction Cosign Matrix from the current Euler Angles of the Device. Multiply Gimbal * Device Matrix = New Device Values with Respect to Gimbal. Find the difference between Gimbal and -(New Device Values). Convert back to Euler Angles
This seems to work for high Roll angles (providing corret Pitch and Yaw to properly compensate) but not for high Pitch and Yaw... Any Thoughts?