Kalman filter for lego mindstorms nxt

One thing that becomes glaringly clear when starting robotics is that nothing goes as expected. The motors don't go as instructed and the sensors don't read as expected. This is a fundamental problem one faces in real world systems where noise is abundant and randomness is part of life. For example, when a motor is rotated by certain degree, it is never guaranteed to land on the desired position due to several physical uncertainties. Rather, it lands on a so call 'cloud of probability distribution'. Which basically means it will land on any point surrounding the desired point with certain probability. That's more of a gambling than precise control.

If one continues running the system based on some ideal model, these errors quickly accumulate and very soon you have a deceivingly wrong understanding of what the state of the system is (it's called system drift). One way to counter this and prevent the error from growing high is obviously to have some feedback of the system state from an independent observation. That's where sensors come in handy.

Unfortunately, as life would be, sensors themselves are not perfect and have the same problem of reading from a cloud of probability. The measurements are seldom the actual state. Instead, it is something close to the actual value depending on whatever dice reading god happens to have at that point in time. The dice readings, of course, is governed by the measurement probability cloud involved in that particular sensor setup.

Depending on how much you paid for the motors and sensors, the clouds will be smaller or larger. But irrespectively, some clouds will always be there and one of the goals of many robot enthusiasts is to deal with these clouds.

One savior in this department is a famous (or infamous, depending on how it goes with you :)) Kalman Filter. This algorithm tries to maintain a good estimate of current state based on two important knowledge of the dynamic system; 1) the model of the actuation and 2) the measurement readings. In ideal world, both of them will determine the exact state independently, but in real world, random errors make them hard to use directly. The best is to use them together in some form of statistically averaging way. Kalman Filter is one such way of doing that.

I have implemented standard kalman filter for lego mindstorms nxt and the source code is available in libnxt library. It's done in NXC language and runs on the standard firmware. It's a flexible implementation using integer matrix algebra and can be used comfortably for one, two, or three dimentional state vectors (above that, you could still use it, but it becomes very slow on nxt hardware).

To use it in meaningful way, it's useful to know how kalman filter works (but not necessarily to the extend of knowing every details). Understanding the model at least will help provide the right parameters for the filter. I found this excellent article explaining Kalman Filter that I felt was very well written. It explains it for one scaler variable, but can be easily applied to vectors using matrix operations.

Cutting long story short, here is how it can be summarized. Kalman Filter works by maintaining the state of the system based on its actuation model and at the same time taking weighted contribution from the measurement. The weight, officially known as Kalman Gain, is determined from an understanding of how accurate the measurement is compared to the accuracy of the current estimate (which is known as Posteriori error covariance). Highly inconsistent measurements have lower weight and therefore contributes less to the final estimate.

It introduces two important assumptions in kalman filter. First is that the system must be linear. That's because the understanding of accuray above is actually computed by simple linear comparison. If the system is not linear, the understanding itself will be flawed. If the system is not linear, there are ways to linearize the system or some other modified/improved form of Kalman filter is used, such as Extended Kalman Filter or Unscented Kalman Filter. In my opinion it's just better to skip Mr. Kalman at that point and go for other versatile algorithms such as Particle Filter

The second assumption in kalman model is that the error introduced is Gaussian distribution, also known as white noise. This is again because it only maintains one estimate of the state which it tries to converge based on the fact that statistically most likely estimate is the actual state, and as one go further away from the actual state, the less likely they happen. This is the basis of many naturally occurring random errors. For example, if you throw 1000 darts at a target, close to bulls eye is where you find most of the darts landing and it becomes thinner as you look further away from bulls-eye. If the error or uncertainty involved is not approximately Gaussian distribution, or not uni-modal (that is, it has more than 1 most-likely estimates), then go for more versatile (but expensive) algorithms such as Particle Filters

The following two equations describe the kalman model; a linear system with white noise introduced. The first is Actuation model and the second is the Observation or measurement model





The actuation model describes that current state is derived from previous state (transformed linearly by function ) and adding whatever input control was applied to the system. is obviously the transformation function that converts control input to the its consequence in state space. is the dreaded white noise which has mean 0 and covariance .

The observation model is described by observation (the observed parameter) being made off current state . is the function that maps system state to the observation parameter. is the white noise added in .

Then the filter steps in a continuous loop. Each step is a time step (represented by t in the equations) and has three phases -- predict phase, intermediate phase and update phase.

Predict phase, given by the following 2 equations, is when it determines the state of the system (called Priori estimate, indicated by the 'hat') based on its actuation model.

Predicted state:



Predicted estimate covariance:



The intermediate phase is where it determines the understanding of accuracy and , called residuals, and the best Kalman Gain for that step.

Innovation or measurement residual:



Innovation (or residual) covariance:



Optimal Kalman gain:



And lastly update phase is where it uses the Kalman Gain to update the Priori estimate making it Postariori estimate , which is the final estimate of the step.

Updated state estimate:



Updated estimate covariance:



All along, there is another paramenter that goes together with the state estimate, called estimate covariance , that maintains the accuracy of state estimate.

The biggest challenge for me was in implementing integer matrix operations and maintaining Kalmain Gain. While kalman equations where quite straight forward (just a bunch of matrix operations), doing it in integer form was a different beast. The dynamics of the equations were so large that holding them within integer range was non-trivial. I had to go with several 'tweaks' to make it work. The first tweak is about holding a matrix inverse in integer form. It's explained in detail at libnxt matrix documentation. The second tweak was with Kalman Gain. This parameter is a fractional parameter that ranges from 0 to 1.0 and must maintain a good precision especially close to 0 for the filter to converge correctly around the set point. On top of that NXC doesn't support function calls recursion so it was another fight to implement matrix co-factor computation.

Here is a video running a simulation on nxt brick: