Kalman Filter Pt. 2

Now that I have some upgrades on the RC truck project to be able to read motor ticks and wheel ticks, I decided it was time to revisit the Kalman filter. Below is a brief overview of the hardware and electronics. There will be more on these upgrades later to come. For now though the important thing to note is that the motor displacement is available with 36 ticks / rev of resolution and the wheels 7 ticks / rev.

This post will be more about some of the errors I made in trying to more rigorously develop a Kalman filter for this application. Some of these errors were fairly basic and others were more nuanced. There is still plenty that can be done to further improve, but the results so far appear to be quite good. If you’re interested more on the theory of Kalman filters, I’d highly suggest checking out this excellent resource.

Design

The state variable

For this filter, use raw ticks to represent distance. Therefore the ticks are the measurement; velocity & accel will be the estimated "hidden variables. I used the common variable names for rotational motion (theta, omega, and alpha) just to avoid confusion between the generic state vector X and x, v, a typically used for linear motion.

I chose the raw ticks in this instance because that is what is being measured. As opposed to applying any sort of “speed calculation” looking at the ticks and time-between ticks or any sort of average, I decided to let the Kalman filter make that estimate. After all, estimating hidden variables is a huge part of the power of the Kalman filter.

Initialize state covariance

Assuming that the vehicle is always started from a standstill, we're very confident in the initial state. However in order to generalize the filter a bit more, I’ll set the initial uncertainty (covariance matrix) based on some rules of thumb. The maximum speed of this motor is 6000 rpm, so a guess for the variance here could be

Using a 2-dimensional filter, P is no longer just the variance of a single state estimate, but rather the covariance or correlated variance matrix. By setting the off-diagonal values to zero, this implies that there is no relationship between position, speed, and acceleration. This is of course not true, but the beauty of the Kalman filter is that this covariance is automatically computed and updated online as part of the algorithm!

Define process model

Though I haven’t stated it yet, this filter is a very simple one. It is a 2-dimensional filter using a kinematic process model. Toward the end I’ll mention some ways in which this could be improved, but for now it’s good enough. Amazingly, using this kinematic model simply assumes a constant acceleration between each time step, but still produces quite nice results. For this particular design, the filter is running online at nominally 100 Hz (this is important to come back to)! For 100 Hz we get a time step of 10 ms.

Using this information, we can build the state transition function F. For this classic Kalman filter, the “transition function” is just a matrix.

The process noise

I wish I had a more robust explanation of how I derived the process noise, but this is one of the most challenging parts of designing a Kalman filter. For this particular case I just assumed that the process noise is discrete time, white noise, Wiener process and used a handy function from the FilterPy library to compute it. While this gave me some starting values for the matrix, it still took some handing tuning of Phi, the spectral density, as an “engineering factor” in order to produce a stable filter with reasonable results. More on that in the next section.

Before jumping into the tuning of the filter, it’s worth noting that the process noise for this particular filter must be rather large. This is a system with a control input (the throttle), which is completely being ignored, despite the fact that we can directly measure that signal and the subsequent state variable (motor current/torque). While I plan to further develop this implementation, these sort of observations really emphasize for me the beauty and power of the Kalman filter. Despite ignoring this information, we still get some pretty great results!

Testing & Debugging

There were a number of issues that I found. I’ll run through them here. Some are probably obvious, at least one is somewhat interesting.

State to Measurement Transform (H-Function)

The explanation above gives the correction H-function to transform the state space into the measurement space. Originally I had this equation inverted which lead to a horrifically diverging filter.

Incorrect H-function leads to possibly the worst Kalman filter.

Non-stationary Time Step

This was probably the most interesting, real-world bug I came across where the real world challenge present unique challenges not always directly addressed in the theory. The Kalman filter in the most basic implementation assumes that the time step is fixed. This is baked into the “predict” step where the filter uses the process model to guess where the system should be and then peeks at the measurement to correct. The problem here is that the Raspberry Pi, a Linux-based SBC, isn’t so great when it comes to millisecond level precision (the control loop runs at a 10 ms time step). It turns out that there is a huge amount of variation from loop-to-loop. The false, fixed time-step assumption results in the following excrement.

Process Noise

The process noise matrix initially was more or less just a simple guess with only diagonal elements. There are more complex methods to derive Q (the process noise matrix) using a strong foundation in signal theory. I elected for the “roll some dice and fix it later method. Later turned out to be sooner than expected. Here are the results with an arbitrary Q-matrix. The filter doesn’t immediately explode, but I went from expulsion to a simple “F” grade. While the signal is smooth, notice the incredibly high delay.

At this point I switched to using some library functions to make a better guess of Q. The results looked much worse, but the parameters used to generate Q were chosen via the aforementioned “roll the dice and fix it later” method.

In order to tune the Q-matrix, I applied a simple scaling factor and adjusted until most of the residuals (the difference between the prediction and the measurement), fell within the bounds of the variance. This is sort of a simple check if the assumptions of the Kalman filter theory are holding.

Future Work

So the filter seems to be working reasonably well. I can use the output for closed loop motor speed control and the system is relatively stable. There are however several basic steps that I can take to further improve upon this simple design.

Filter scoring

NEES is one method for scoring the performance of the filter. There are many others as well. These would be more objective methods vs. the classic “eye ball it” approach taken here.

Automated tuning

There are plenty of optimization algorithms that could look at the performance of the filter vs. other offline filtering methods like the smooth-kf post processing methods (such as the RTS smoother) or filtfilt. These optimization methods could look at the scoring from multiple objectives and try to tune Q and R.

More realistic measurement noise

Right now the measurement noise is a fixed value. From the previous experiments I know that this is not the case. I could improve the performance of the filter by adding in a dynamic R as opposed to fixed R. Similarly, Q could be recomputed at each time step based on the true dt (since it's an arg to the white noise method), but it's unclear if this is worth it or if the computational cost is available.

Better process model

Right now there is only a kinematic process model implemented. A dynamic process model would take into account the wheel moments of inertia as well as the actual motor torque produced. In order for this to become very accurate, online battery voltage monitoring would be needed.