Drift correction

To make a useful tracking system, the drift error (9.3) cannot be allowed to accumulate. Even if the gyroscope were perfectly calibrated, drift error would nevertheless grow due to other factors such as quantized output values, sampling rate limitations, and unmodeled noise. The first problem is to estimate the drift error, which is usually accomplished with an additional sensor. Practical examples of this will be given in Section 9.2. For the simple merry-go-round example, imagine that an overhead camera takes a picture once in a while to measure the orientation. Let $\hat{\theta}_d[k]$ denote the estimated orientation from this single sensor measurement, arriving at stage $k$.

Because of drift error, there are now two conflicting sources of information: 1) The orientation $\hat{\theta}[k]$ estimated by integrating the gyroscope, and 2) the orientation $\hat{\theta}_d[k]$ instantaneously estimated by the camera (or some other, independent sensor). A classic approach to blending these two sources is a complementary filter, which mathematically interpolates between the two estimates:

$$\hat{\theta}_c[k] = \alpha \hat{\theta}_d[k] + (1-\alpha) \hat{\theta}[k] ,$$ (9.10)

in which $\alpha$ is a gain parameter that must satisfy $0 < \alpha < 1$. Above, $\hat{\theta}_c[k]$ denotes the corrected estimate at stage $k$. Since the gyroscope is usually accurate over short times but gradually drifts, $\alpha$ is chosen to be close to zero (for example, $\alpha = 0.0001$). This causes the instantaneous estimate $\hat{\theta}_d[k]$ to have a gradual impact. At the other extreme, if $\alpha$ were close to $1$, then the estimated orientation could wildly fluctuate due to errors in $\theta_d[k]$ in each stage. An additional consideration is that if the sensor output $\hat{\theta}_d[k]$ arrives at a much lower rate than the gyroscope sampling rate, then the most recently recorded output is used. For example, a camera image might produce an orientation estimate at $60$Hz, whereas the gyroscope produces outputs at $1000$Hz. In this case, $\hat{\theta}_d[k]$ would retain the same value for 16 or 17 stages, until a new camera image becomes available.

It is important to select the gain $\alpha$ to be high enough so that the drift is corrected, but low enough so that the user does not perceive the corrections. The gain could be selected ``optimally'' by employing a Kalman filter [45,142,160]; however, the optimality only holds if we have a linear stochastic system, which is not the case in human body tracking. The relationship between Kalman and complementary filters, for the exact models used in this chapter, appears in [120].

Using simple algebra, the complementary filter formulation in (9.10) can be reworked to yield the following equivalent expression:

$$\hat{\theta}_c[k] = \hat{\theta}[k] - \alpha \hat{d}[k]$$ (9.11)

in which

$$\hat{d}[k] = \hat{\theta}_d[k] - \hat{\theta}[k].$$ (9.12)

Above, $\hat{d}[k]$ is just an estimate of the drift error at stage $k$. Thus, the complementary filter can alternatively be imagined as applying the negated, signed error, by a small, proportional amount $\alpha$, to try to incrementally force it to zero.