Integration

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

$$\hat{\omega}[k] = (\hat{\omega}_x[k],\hat{\omega}_y[k],\hat{\omega}_z[k])$$ (9.14)

arrives from the sensor. In Section 9.1, the sensor output $\hat{\omega}[k]$ was converted to a change $\Delta\theta[k]$ in orientation. For the 3D case, the change in orientation is expressed as a quaternion.

Let $q(v,\theta)$ 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 $\hat{\omega}[k]$ is the sensor output at stage $k$, then the estimated rotation axis is

$$\hat{v}[k] = \hat{\omega}[k]/\Vert\hat{\omega}[k]\Vert .$$ (9.15)

Furthermore, the estimated amount of rotation that occurs during time $\Delta t$ is

$$\Delta\hat{\theta}[k] = \Vert\hat{\omega}[k]\Vert \Delta t .$$ (9.16)

Using the estimated rotation axis (9.15) and amount (9.16), the orientation change over time $\Delta t$ is estimated to be

$$\Delta \hat{q}[k] = q(\hat{v}[k],\Delta\hat{\theta}[k]) .$$ (9.17)

Using (9.17) at each stage, the estimated orientation $\hat{q}[k]$ after obtaining the latest sensor output is calculated incrementally from $\hat{q}[k-1]$ as

$$\hat{q}[k] = \Delta \hat{q}[k] * \hat{q}[k-1] ,$$ (9.18)

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).