Now consider the problem converting the sequence of gyroscope outputs into an estimate of the 3D orientation. At each stage a vector
arrives from the sensor. In Section 9.1, the sensor output
was converted to a change
in orientation. For the 3D case, the change in orientation is expressed as a quaternion.
be the quaternion obtained by the axis-angle conversion formula (3.30). Recall from Section 8.1.2 that the instantaneous axis of rotation is the magnitude of the angular velocity. Thus, if
is the sensor output at stage , then the estimated rotation axis is
Furthermore, the estimated amount of rotation that occurs during time is
Using the estimated rotation axis (9.15) and amount (9.16), the orientation change over time is estimated to be
Using (9.17) at each stage, the estimated orientation
after obtaining the latest sensor output is calculated incrementally from
in which denotes quaternion multiplication. This is the 3D generalization of (9.9), in which simple addition could be used to combine rotations in the 2D case. In (9.18), quaternion multiplication is needed to aggregate the change in orientation (simple addition is commutative, which is inappropriate for 3D rotations).
Steven M LaValle