Tilt correction

The drift error $d(t)$ in (9.3) was a single angle, which could be positive or negative. If added to the estimate $\hat{\theta}(t)$, the true orientation $\theta(t)$ would be obtained. It is similar for the 3D case, but with quaternion algebra. The 3D drift error is expressed as

$$d(t) = q(t) * \hat{q}^{-1}(t) ,$$ (9.19)

which is equal to the identity rotation if $q(t) = \hat{q}(t)$. Furthermore, note that applying the drift error to the estimate yields $q(t) = d(t) * \hat{q}(t)$.

Since the drift error is a 3D rotation, it could be constructed as the product of a yaw, pitch, and a roll. Let tilt error refer to the part of the drift error that corresponds to pitch and roll. This will be detected using an ``up'' sensor. Let yaw error refer to the remaining part of the drift error, which will be detecting using a ``compass''. In reality, there do not exist perfect ``up'' and ``compass'' sensors, which will be addressed later.

Figure 9.3: If ``up'' is perfectly sensed by an accelerometer that is rotated by $\theta$, then its output needs to be rotated by $\theta$ to view it from the world frame.

Suppose that a sensor attached to the rigid body always reports an ``up'' vector that is parallel to $y$ axis in the fixed, world coordinate frame. In other words, it would be parallel to gravity. Since the sensor is mounted to the body, it reports its values in the coordinate frame of the body. For example, if the body were rolled $90$ degrees so that its $x$ axis is pointing straight up, then the ``up'' vector would be reported as $(0,0,1)$, instead of $(0,1,0)$. To fix this, it would be convenient to transform the sensor output into the world frame. This involves rotating it by $q(t)$, the body orientation. For our example, this roll rotation would transform $(0,0,1)$ into $(0,1,0)$. Figure 9.3 shows a 2D example.

Figure 9.4: (a) Tilt error causes a discrepancy between the $y$ axis and the sensed up vector that is rotated using the estimate $\hat{q}[k]$ to obtain $\hat{u}$. (b) The tilt axis is normal to $\hat{u}$; a rotation of $-\hat{\phi}$ about the tilt axis would bring them into alignment, thereby eliminating the tilt error.

Now suppose that drift error has occurred and that $\hat{q}[k]$ is the estimated orientation. If this transform is applied to the ``up'' vector, then because of drift error, it might not be aligned with the $y$ axis, as shown Figure 9.4. The up vector $\hat{u}$ is projected into the $xz$ plane to obtain $(\hat{u}_x,0,\hat{u}_z)$. The tilt axis lies in the $xz$ plane and is constructed as the normal to the projected up vector: $\hat{t}= (\hat{u}_z,0,-\hat{u}_x)$. Performing a rotation of $\phi$ about the axis $\hat{t}$ would move the up vector into alignment with the $y$ axis. Thus, the tilt error portion of the drift error is the quaternion $q(\hat{t},\hat{\phi})$.

Figure 9.5: (a) There is no gravity sensor; the accelerometer measures the vector sum of apparent acceleration due to gravity and the true acceleration of the body. (b) A simple heuristic of accepting the reading as gravity only if the magnitude is approximately $9.8$m$^2$ will fail in some cases.

Unfortunately, there is no sensor that directly measures ``up''. In practice, the accelerometer is used to measure the ``up'' direction because gravity acts on the sensor, causing the sensation of upward acceleration at roughly $9.8$m/$s^2$. The problem is that it also responds to true linear acceleration of the rigid body, and this cannot be separated from gravity due to the Einstein equivalence principle. It measures the vector sum of gravity and true linear acceleration, as shown in Figure 9.5. A simple heuristic is to trust accelerometer outputs as an estimate of the ``up'' direction only if its magnitude is close to $9.8$m$^2$ [76]. This could correspond to the common case in which the rigid body is stationary. However, this assumption is unreliable because downward and lateral linear accelerations can be combined to provide an output magnitude that is close to $9.8$m$^2$, but with a direction that is far from ``up''. Better heuristics may be built from simultaneously considering the outputs of other sensors or the rate at which ``up'' appears to change.

Assuming that the accelerometer is producing a reliable estimate of the gravity direction, the up vector $\hat{u}$ is calculated from the accelerometer output $\hat{a}$ by using (3.34), to obtain

$$\hat{u}= \hat{q}[k] * \hat{a}* \hat{q}[k]^{-1} .$$ (9.20)