Source to learn Kalman Fusion, explanatory code snippets

Question

Currently I am reading a book of Mr. Thrun: Probabilistic Robotics. I find it really helpfull to understand concept of filters, however I would like to see some code in eg. Matlab. Is the book "Kalman Filter for Beginners: with MATLAB Examples" worth buying, or would you suggest some other source to learn the code snippets from?

Answer 1

I have read the book, and found it unnecessarily obtuse. Unfortunately, the code snippets will not be very helpful, since they will probably look exactly like the equations, while using a matrix library like Eigen, OpenCV, boost, or just Matlab. To get a good understanding of a Kalman Filter, you should start with a review of multi-variate Gaussian random variables, then brush up on Taylor Series expansions, then realize we are just inventing a covariance matrix for the joint distribution of measurements and state.

For example, here's some code I wrote for a robotic rover which uses an EKF.

---

void DifferentialKinematics::ekfMeasurement(Eigen::MatrixXd&X,Eigen::MatrixXd&Q,Eigen::MatrixXd&Z,Eigen::MatrixXd&R){
  Eigen::MatrixXd I=Eigen::MatrixXd::Identity(3,3);
  Eigen::MatrixXd ZP = Eigen::MatrixXd(Z);
  Eigen::MatrixXd XP = Eigen::MatrixXd(X);
  Eigen::MatrixXd z = Z-X;
  Eigen::MatrixXd S =Q+R;

  Eigen::MatrixXd K=Q*S.inverse();

  X = X + K*(z);
  Q = (I-K)*Q;
}
/**
 * Update given wheel speeds left, right.
 * Both are in/out
 */
void DifferentialKinematics::ekfPredict(float left, float right, Eigen::MatrixXd&pose, Eigen::MatrixXd&cv){
  float hth=pose(2)+getTheta(left,right)/2;
  float dd = std::fabs(getVel(left,right));
  updatePose(left,right,pose);

  //Form Jacobian
  Eigen::MatrixXd V=Eigen::MatrixXd::Zero(3,2);
  V(0,0)=(cos(hth)+(dd/width)*sin(hth))/2;
  V(0,1)=(cos(hth)-(dd/width)*sin(hth))/2;
  V(1,0)=(sin(hth)-(dd/width)*cos(hth))/2;
  V(1,1)=(sin(hth)+(dd/width)*cos(hth))/2;
  V(2,0)=(-1/width);
  V(2,1)=(1/width);

  Eigen::MatrixXd G;
  G=Eigen::MatrixXd::Identity(3,3);
  G(0,2)=-dd*sin(hth);
  G(1,2)=cos(hth)*dd;

  Eigen::MatrixXd M=Eigen::MatrixXd::Zero(2,2);
  M(0,0)=left < 0? -1*left*kl:left*kl;
  M(1,1)=right < 0? -1*right*kr:right*kr;

  Eigen::MatrixXd R=V*M*V.transpose();
  Eigen::MatrixXd r2 = G*cv*G.transpose()+R;
  cv = r2; 
  return;
}

Answer 2

The two links that I found most useful were Kalman Filter for Undergrads1 and Kalman Filter for Dummies. They're not high on the theory though. and Student Dave's Kalman Filter Tutorial. The last one has matlab code that you can play with and is easy to follow.

Answer 3

If you are looking for a source to get an intuitive feeling for what the Kalman Filter is actually doing I would suggest going through lesson 2 of Udacity's Artificial Intelligence for Robotics course found here: https://www.udacity.com/course/cs373.

The course is free and it has video lectures and simple code examples. The explanation given is a little simplistic but it does help to build an intuition for how and why the filter works.

Answer 4

Once you have a real intuition for the Kalman equations you will easily be able to translate the equations / models into code. I highly recommend working (by hand) the examples in the Lecture Subject MI63: Kalman Filter Tank Filling - Kalman Filter Applications.

The examples highlight how the system model can greatly affect the output of the Kalman filter and provide concrete examples of tracking random variables.

More information on the derivation of the models used in the examples is given in UNC-Chapel Hill, COMP 145 - Team 18: The Kalman Filter Learning Tool - Dynamic and Measurement Models.

You can implement the examples in something as simple as Excel or Matlab. However, if you plan on implementing the Kalman equations in C++, I recommend use the Eigen software package like Josh previously mentioned.

Answer 5

I've been collecting books on Kalman filtering and target tracking for some years now.

Most approachable book I've found: Gelb's Applied Optimal Estimation. It is widely considered a classic in the field. It is also relatively inexpensive.

Second place goes to Brookner's Tracking and Kalman Filtering Made Easy. It is aimed primarily at radar processing. He goes to a good bit of trouble to explain simpler tracking filters first, then shows that the Kalman filter is nothing magic, just a more flexible way of coming up with the coefficients for the filter.

Neither of them tackle the association problem for multiple target tracking. Blackman's Multiple Target Tracking with Radar Applications is a good start. New, it is expensive, like all Artech House books. Amazon says there are some inexpensive used copies available.

Unfortunately, I have yet to find anything that even remotely resembles an approachable explanation for Reid's Multiple Hypothesis Tracker, which you need if you are trying to do multiple-target tracking. This is a robust approach to the problem of associating sensor reports with existing tracks and/or initiating new tracks if something new pops up. You NEED to solve the association problem to do multiple target tracking. Unfortunately, Reid's paper is almost incomprehensible, at least to me. (Yes, there are simpler methods, but they are not nearly as robust.)

Answer 6

The Kalman Filter is based on some assumptions - a linear process with independent normally distributed noise. Noise over time is independent. Each measurement is also independent.

The Wikipedia article (http://en.wikipedia.org/wiki/Kalman_filter) is quite complete in information on this. This article should be enough to allow you to implement the Kalman Filter. Other sources also exist on the internet. Most people here will not be able to give an opinion on the book you mentioned - since we have not read it, and I will never read it because I do not need to.

The Wikipedia article contains, in particular, detailed equations showing each step that a Kalman filter takes. In particular the equations given implement a filter allowing the covariance (noise in measurements) to differ on different timesteps, and keeps an updated estimate of the covariance (accuracy) of the estimated states.

There is also an example which helps walk through the equations.

An alternative way of implementing the Kalman filter is simply as a fixed state-space linear time-invariant filter. The relevant equation is $x(k)=x(k-1)+Ky(k)$. This is a simplification of the full set of equations, because $K$ - the Kalman gain, is fixed if the covariances are fixed. The gain can also be obtained from the solution to the Discrete-time Algebraic Ricatti equation.

Answer 7

I'm biased (since I have TA'd his course twice), but I think Prof. Marin Kobilarov's derivation of the KF/EKF is far superior to Thrun's. I have read almost every book on KF/EKFs mentioned in previous answers, and I think Marin's lecture notes are excellent for the theoretical aspects of optimal state estimation. Thrun's presentation is far from the worst, though. The beauty of Marin's approach is that he develops the KF measurement updates from an optimization standpoint: he starts with a simple least-squares approach and goes through the necessary and sufficient conditions for optimality for both linear and nonlinear measurement models.

The course website also has some easily-digestible matlab code: https://asco.lcsr.jhu.edu/en530-603-f2017-applied-optimal-control/

Lecture 10 is specifically about optimal state estimation, namely the KF/EKF: https://asco.lcsr.jhu.edu/docs/EN530_603_F2015/lecture10.pdf

PS. During my orals/quals, I was asked to derive the EKF by hand from memory. If I hadn't learned the above approach, which I find very intuitive, I think I would have been lost in mathematical minutia. As always, YMMV.