Sensor outputs usually arrive at a regular sampling rate. For example, the Oculus Rift gyroscope provides a measurement every $ 1$ms (yielding a $ 1000$Hz sampling rate). Let $ \hat{\omega}[k]$ refer to the $ k$th sample, which arrives at time $ k \Delta t$.

The orientation $ \theta(t)$ at time $ t = k\Delta t$ can be estimated by integration as:

$\displaystyle \hat{\theta}[k] = \theta(0) + \sum_{i=1}^k \hat{\omega}[i] \Delta t .$ (9.8)

Each output $ \hat{\omega}[i]$ causes a rotation of $ \Delta\theta[i] = \hat{\omega}[i] \Delta t$. It is sometimes more convenient to write (9.8) in an incremental form, which indicates the update to $ \hat{\theta}$ after each new sensor output arrives:

$\displaystyle \hat{\theta}[k] = \hat{\omega}[k] \Delta t + \hat{\theta}[k-1].$ (9.9)

For the first case, $ \hat{\theta}[0] = \theta(0)$.

If $ \omega(t)$ varies substantially between $ \theta(k \Delta t)$ and $ \theta((k+1)\Delta t)$, then it is helpful to know what $ \hat{\omega}[k]$ corresponds to. It could be angular velocity at the start of the interval $ \Delta t$, the end of the interval, or an average over the interval. If it is the start or end, then a trapezoidal approximation to the integral may yield less error over time [133].

Steven M LaValle 2016-12-31