What are methods for dealing with compass lag (rate dependent hysteresis)?

I've got a tread-driven robot, with low precision wheel encoders for tracking distance and an electronic compass for determining heading. The compass has significant (> 1 second) lag when the robot turns quickly, e.g. after reaching a waypoint — pivoting in place to point to its new heading.

What are ways for dealing with the lag? I would think one could take a lot of measurements and model the compass response. However, this seems problematic since it's rate-dependent and I don't know the instantaneous rate.

As a simple-but-slow approach, I have the robot turn until it's very roughly pointed in the right direction, then make very small incremental turns with brief measurement pauses until it's pointed the right way. Are there other ways of dealing with this?

The lag in the compass is because of a low-pass filter, to suppress high frequency noise.

• There exist more expensive magnetometers which have less noise, and therefore, less lag.
• It is also possible to use a gyroscope to improve accuracy. In fact, this is what Inertial Measurement Units (IMUs) do. This can be accomplished by using a Kalman filter. Improving accuracy helps to decrease lag, because increased accuracy reduces the dependency on a low pass filter to suppress noise. The Kalman filter fuses the data from the magnetometer, and also the gyroscope (which measures rate of change in heading).

If you stick with your current compass, there are two possible solutions (Warning, this does get increasingly advanced, but option 1 should be accessible to most people without too much work).

1. You can try to cancel out the filter. This can remove lag, but also increases high frequency noise. After doing this, you can try to control the robot based on the new estimate of heading. To do this, you must experiment to work out the low pass filter parameters. For example, in discrete time, you might find:

$$\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$$ where $\hat\theta(t)$ is the estimated heading (compass output) at time $t$, $\theta$ is the actual heading (ground truth) at time $t$.

You can find the parameters $a_i$ by doing an experiment where you measure the ground truth using some other external means. Given $n+k+1$ samples, you have this equation: $$\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$$

And you can solve by finding: $$\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$$ where $M^+$ is the pseudo-inverse matrix of $M$. There is no definitive way to work out $k$, so you will probably just guess. For bonus points, this assumes that the noise is white and independent, but you can whiten it first to remove bias, and therefore improve your estimate of the parameters.

You can convert this to a transfer function (also known as Z-transform in the discrete time domain):

$$\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$$

To cancel this out, we can take the inverse (where $\bar\theta(t)$ is our new estimate of heading):

$$\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$$

Converting back to the time domain:

$$a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$$

$$\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$$

we can then use $\bar\theta$ to control the robot.

This will be very noisy, so you might want to still put $\bar\theta$ through a low-pass filter before use (although perhaps one with less lag).

2. The above solution is still not the best way. The noisy estimate may not be very useful. If we put this into a state space equation, we can design a Kalman filter, and a full-state feedback controller using LQR (linear quadratic regulator). The combination of a Kalman filter and LQR controller is also known as an LQG controller (linear quadratic gaussian), and use loop-transfer recovery to get a good controller.

To do this, come up with the (discrete-time) state-space equations:

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$, $\vec{y}(t)=C\vec{x}(t)$

or: $$\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]= \left[\matrix{ A_1&A_2&\cdots&0&0&0\\ 1&0&\cdots&0&0&0\\ 0&1&\cdots&0&0&0\\ \vdots&\vdots&&\vdots&\vdots&\vdots\\ 0&0&\cdots&1&0&0\\ 0&0&\cdots&0&1&0 }\right] \vec{x}(t-1) + \left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$$

$$\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$$

where $\vec{u}(t-1)$ represents the power in the motors to turn the robot, and $A_0$, $A_1$, $B_0$, $B_1$ is how much it affects the heading based on position and speed (you can choose non-zero values for the other elements of the $B$ matrix, and first row of the $A$ matrix too).

Then, you can build your observer (Kalman Filter), by choosing noise estimates $Q_o$ and $R_o$ for the process noise and measurement noise. The Kalman Filter can then find the optimal estimate of heading, given those assumptions about the noise. After choosing the noise estimates, the implementation just depends on implementing code for the Kalman Filter (equations can be found on Wikipedia, so I won't go over it here).

After that, you can design an LQR controller, this time, choosing $Q_c$ and $R_c$ representing the weighting given to regulating the heading, and trying to limit the use of the actuators. In this case, you might choose $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ and $R_c = \left[1\right]$. This is done because LQR finds the optimal controller to minimise a cost function: $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

Then, you just put it through the discrete time algebraic Riccati equation:

$$P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$$

and solve for a positive definite matrix $P$.

Thus, your control law can be given by:

$$\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$$

where $K = (R + B^T P B)^{-1}(B^T P A)$

Finally, just doing this won't work very well, and is likely to be unstable because of the noise. Indeed, that means option 1 probably won't work unless you first put $\bar\theta$ through a low-pass filter (albeit not necessarily with such a long effective lag time). This is because while LQR is guaranteed stable, as soon as you use a Kalman filter, the guarantee is lost.

To fix this, we use the Loop Transfer Recovery technique, where you adjust the Kalman filter, and instead choose a new $Q_o = Q_0 + q^2BVB^T$, where $Q_0$ is your original $Q$ matrix, tuned so that the Kalman filter is optimal. $V$ is any positive definite symmetric matrix, which you can just choose as the identity matrix ($V=I$). Then, just choose a scalar $q$. The resulting controller should become (more) stable as $q \rightarrow \infty$, although the $Q_o$ matrix becomes de-tuned, which means it becomes less optimal.

Therefore, you just increase $q$ until it is stable. Another way you can try to make it stable, is to increase $R_c$ (or decrease $Q_c$) to make the LQR controller slower.

The concepts in this post does get quite advanced, but if you need to solve things like the Riccati equation, you can use MATLAB or other software to do this. There may also be libraries already implementing the Kalman filter (again, I believe MATLAB also does this).

For an embedded application, you may need to implement the Kalman filter yourself, although there is probably a C implementation.

• Thanks for you excellent and in-depth answer. I follow the gist of your first solution and I'm sure I can work through it. The second one, as you say, is more challenging and I'll have to work to see if I can follow it all. – ViennaMike Dec 31 '12 at 21:26

A gyro is the simple answer. I've always heard, gyro for the short measurements, compass for the long. And realistically a cup of kallman filter between the two most of the time. The price of a 6DOF gyro/acc board is less than \$20 these days, far too cheap to not use one.

At one time, I worked through someone else's Kallman filter. and got it working. A kallman filter is actually more of an approach, not a exact implementation, and in the gyro case, the end result does not need to use matrix math. It makes for much simpler code.