Now consider the problem converting the sequence of gyroscope outputs into an estimate of the 3D orientation. At each stage a vector

(9.14) |

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

Let 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 as

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 2020-01-06