Several years ago I read an article in Nuts and Volts magazine on a balance bot. The bot used an accelerometer to measure tilt and a rate gyro to measure the rate of change of that tilt. What grabbed my attention was that it claimed to use a Kalman filter. I am interested in Kalman filters so I took a look. The implementation did not look like anything I was familiar with. In particular the use of a Jacobian and other notational details did not match anything I had seen.
Recently I was scanning through a book on Kalman filters and I finally figured it out. This notation was used in the discussion of the Extended Kalman Filter (EKF) which is a variation used when the system being modeled is non-linear. (I Googled up an interesting paper that provides a good introduction to the Kalman filter.) This code keeps popping up and I have always felt that it had major problems. Now might be a good time to describe them in detail.
The Kalman filter is pretty simple. It estimates the system state using a predictor/corrector scheme. It predicts the state by applying a dynamic model to the previous state. It corrects that estimate by comparing it to measurements related to the state. The hard parts are in the details.
The notation in the C source file leaves something to be desired so here it is in a somewhat more readable form.
State variables:
system
![\left[ \begin{matrix}\dot{x}_1 \\\dot{x}_2\end{matrix} \right] = \left[ \begin{matrix}-1 & 0\\0 & 0\end{matrix} \right]\left[ \begin{matrix}x_1 \\x_2\end{matrix} \right] +\left[ \begin{matrix}gyro \\0\end{matrix} \right]](equ2.gif)
Here we have the first problem. That last term with the gyro measurement should not be there since the system dynamics should depend only on the state variables. The only exception being for control inputs.
This is wrong.
The obvious solution to this is to add a state variable for the angle rate. (later)
The next item documented in the tilt code is the Jacobian. This is supposed to be an array computed by taking the partial derivatives of the state transition function with respect to the system states evaluated at the last estimate of the system states. This ends up being a constant matrix with no state variables with nothing to evaluate. This should be a hint that the system is linear and this is the wrong approach.
I think that is enough to show the major problems. A missing state variable, a measurement in the state propagation, inefficient use of EKF formulation.
Let's start again. This time including the tilt rate and acceleration in the state variables. I will use the same notation as the paper I referenced above. The filter equations are summarized in Figure 1-2.
Besides making the system dynamic equation work this also will improve the solution. The Kalman filter is basically performing a weighted average of the results of the state propagation and the measurements. More measurements always results in lower noise in the output. So adding the angle rate to the filter will reduce the noise.
This system is simple enough that the state transition matrix A can be done by inspection. The general method of doing this is by computing eF(t-t0). Where F is the matrix from the system differential equations. A complicated process and best skipped if you can avoid it.
![A =\left[ \begin{matrix}1 & dt & \frac{dt^2}{2} & 0\\0 & 1 & dt & 0\\0 & 0 & 0 & 1\end{matrix} \right]](equ4.gif)
The next crucial thing to figure out is the matrix H which maps the state variables onto the measurements. (See equation 1.2)
Letting z1 be the angle measurement and z2 be the rate measurement, then:
![H = \left[ \begin {matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 1\end{matrix}\right]](equ5.gif)
Or the angle measurement matches the angle state and the rate measurement is the rate plus bias.
This leaves only two more things to nail down. First up is the measurement noise covariance matrix, R. This will be a 2X2 matrix and since we can assume that the measurements are independent, the two non-diagonal entries will be zero. The other two terms will be the measured variance of the two sensors. The code lists only the noise of the angle measurement and it is much too large. If the angle variance really was 0.3 rad, (2 pi radians (~6.28) equals 360 degrees) it would be completely useless in this application.
The system covariance matrix Q describes the system model noise. I have never seen anything that describes a good way to figure out what should go here. In practice I use it as a system tuning parameter. Higher model noise will result in the filter giving higher weights to the measurements which results in less smoothing but faster response. Since I suspect that most of the "noise" in the system model results from the assumption of the tilt acceleration being constant, that is were I will put all of the noise. So just one entry in the Q matrix will be non-zero.
Because everything used to update the Kalman gains do not change with time, the Kalman gains will converge to constant values. This means that the gains can be precomputed on something with more computational horsepower than the target micro-controller and then simply stored as constants. Eight of them.
I needed some data to run through the filters so I trolled the net looking for some. I finally located a data file on an RC Forum. The data file has a couple of problems. The first is that it is from a three axis sensor system and while it appears that an attempt was made to constrain the motion for this test file to excite a single axis at a time, there still might be some bleed over. The second problem is far worse. The claimed units for the rate gyros is degrees/second. But the data shows a rotation of almost 90 degrees in less than 10 seconds which implies an average rate of over 9 degrees/second. The rates never exceed 1 degree/second so the scaling is obviously wrong. Rather than register for a forum in order to post a question to a dead thread I simply assumed that the rates are in radians/second and corrected them.
In order to test my version of the filter I decided it would be a good time to try a new tool. Scilab is an open source program similar to MATLAB. Both are very good at matrix algebra which is what is needed here. I converted the Rotomotion code to a source file for Scilab and created another for my version.
One problem I am having with the Rotomotion code is the values for process and measurement noise. The value used for the measurement (angle from accelerometer) is 0.3 and from the context (the angle is computed using atan2()) this is in radians. This is a really big value and is much larger than you would get from any sensor. Does this reflect the occasional problem of linear acceleration munging the angle? Or is there no explanation at all? The other problem is the process noise. The comments say: "Q is a 2x2 matrix that represents the process covariance noise. In this case, it indicates how much we trust the accelerometer relative to the gyros." The second sentence is false as Q is actually the covariance of the angle and bias system states. It has nothing to do with the measurements. Given these questions it is hard to know what the performance of this filter with the given parameters really is. In any case the values chosen indicate that the system model is trusted far more than the acceleration measurement.
First is a straight up comparison of the measured angle to the filtered angle.
|
|
Clearly the Rotomotion version is doing a poor job of tracking the angle. A plot of the difference between the measured and filtered angle makes it more obvious.
 |
 |
The differences are quite dramatic for the Rotomotion version while for for my filter the difference resembles zero mean noise. Which is a good thing.
Finally a look at the reason for the filter. Its goal was supposed to be to automagically determine the gyro bias. A few words first about how that is done.
A Kalman filter compares the predicted values of the system state to measurements. Differences between these get fed back in as corrections using the Kalman gains. Consider for a moment a system that isn't moving. So long as the system states reflect the measurements the errors will be mostly from sensor noise and have a zero mean. But if the gyro is biased, the errors will not be zero mean and the system state will be driven to reflect that. Eventually the bias system state will reach a value that brings the errors back to zero mean. So changes in the bias system state are driven by differences between the measurements and the system states.
 |
 |
Here the differences are more subtle. Both curves have about the same shape but significantly, the Rotomotion version has a much larger range. Which means that the bias responds too quickly and while it might track well during periods with no dynamics, as soon as something happens it goes off the rails. My version is also affected by dynamics but not to as large an extent.
Finally, the filtered rate. Note that while the Rotomotion code provides an unbiased rate, it doesn't filter it. My version does.
 |
 |
The filtered version is much less noisy. But the magnitude has also been reduced. I can think of a couple of reasons for this. The first is that the rate gyro is much noisier than the accel so the gains for the gyro data will be lower. The other is that my guess at fixing the gyro data was wrong.
The Rotomotion paper made much of optimizing the code for speed so a comparison is probably in order.
| Rotomotion | Improved | |
|---|---|---|
| Additions | 16 | 14 |
| Multiply | 4 | 8 |
| Multiply by dt | 12 | 3 |
| Division | 2 | 0 |
Even though my code has twice as many system states and measurements, it requires less computation because the gains are pre-computed. I broke out the instances where there is a multiplication by dt. If you design the system so that this is a power of two and use fixed point math these can be replaced with bit shifts.
One last weirdness in the Balance Bot code. Besides using the rate gyro measurement to propagate the system state, it runs the state propagation (predictor) at a different rate than the state update. (corrector) Every reference on Kalman filters I have ever seen always runs these at the same rate. There are a couple of references to a Kalman filter paper scattered in the code but no citation so I can't look it up. (Note: the actual Balance Bot hardware takes the measurements at the same rate. Which makes the provision in the code for running them at separate rates even more mysterious.)
If the rate gyro measurements are really available at a higher rate than the accelerometer, the usual method of handling this is to integrate the rate gyro measurements. Then when an acceleration measurement is available you do a Kalman filter step. Now of course you no longer have a rate measurement but two angle measurements. That is no problem for the Kalman filter. The only change is to the H matrix.
![H = \left[ \begin {matrix}1 & 0 & 0\\1 & 0 & 1\end{matrix}\right]](equ6.gif)
Even though there is no longer a rate measurement I left the angle rate state in the system model. Otherwise the model assumes that the angle is constant. The constant velocity assumption isn't accurate either but it is better.
There is a minor problem in handling the gyro bias which can be handled two ways. Here I have decided to handle it as a lumped value integrated with the angle data. To see the true gyro bias you will have to divide this value by the number of samples in the integration period.
A Balance Bot that simply sits in one spot and manages to stay upright isn't very interesting. But once you add linear motion to it the system dynamics change which requires a more complicated system model than used here.
Home