I'm trying to develop a system with the following characteristics:


  • 3-axis accelerometer [3 DOF]
  • 3-axis gyroscope [3 DOF]
  • GPS with three parameters (lat, lon, altitude) [3 DOF]
  • Barometric pressure [1 DOF] -estimates altitude
  • 3-axis magnetometer [3 DOF]


  • lat, lon
  • altitude
  • velocity (x,y,z)
  • attitude
  • rotation speed

From the very basic research I've done, I think I need a Kalman Filter to fuse the sensor data together. The lat/lon/altitude from the GPS is augmented by the data from the sensors; giving overall better GPS accuracy.

Does anyone know how best to approach this problem and/or if there is any source code available?

Many thanks in advance,


3 Answers 3


Keep in mind that a Kalman Filter is a method for estimating the state of a linear dynamic system. In practice that means given a state vector $x$, you have to design a Matrix $H$ such that the product $y = Hx$ describes your measurements. That is, $y$ would look like

$$ y = \left(\begin{array}{c} latitude \\ longitude \\ \vdots \\ rotation speed \end{array}\right)$$

in your case. After you have found a vector $x$ describing your system state and an appropriate $H$, you can then use the Kalman Filter equations to estimate the state $x$ . While have some freedom in choosing $x,y$ and $H$, this still requires some careful thinking.

An alternative would be to use a non-linear estimator. This makes the problem easier in that you only have to find any, arbitrary function $h$ such that $y = h(x)$ is the measurement vector. (This is the same $y$ as above) The most popular non-linear estimator these days is probably the Unscented Kalman Filter, which has better performance than the EKF under almost every scenario.

In any case, I doubt you will find published literature on this exact problem. I had a similar problem 2 years ago, and the most involved systems I could find had about three sensors or less. However, there is Sebastian Thruns free course "How to build a robotic car" on Udacity, which covers a lot of estimation theory, so I would recommend that if you want to learn a bit more.

  • 1
    $\begingroup$ Unscented Kalman filtering doesn't work well. I suggest to use the 'particle filter' or the 'moving horizon filter'. If you have a lot of time to implement a very good nonlinear filter you can use the 'log homotopy filter' discovered by Daum. $\endgroup$ Sep 12, 2012 at 14:56
  • $\begingroup$ @Benno: I've been reading up on "Probabilistic Robotics" by Sebastian Thrun, Wolfram Burgard and Dieter Fox. I now understand why I should use an unscented kalman filter. I'm going to try and code something simple that I can extend to the 13DOF model. $\endgroup$
    – Eamorr
    Sep 12, 2012 at 16:07
  • $\begingroup$ @Riccardo: While a particle filter can better handle multimodal and other complex or highly non-linear functions, it is also much more expensive to compute. For many "reasonable" measurement functions, an UKF is a good tradeoff between accuracy and speed. (It is basically a particle filter with very few, very carefully placed particles) $\endgroup$
    – Benno
    Sep 13, 2012 at 1:14

Maybe you can use an $EKF$ (extended Kalman filter). You can find papers on 'Transfer alignment'. You can also find hints on the book: 'Introduction to random signals and applied Kalman filtering' by R.G. Brown and P.Y.C. Hwang and also in 'Kalman filtering' by M.S.Grewal and A.P.Andrews. In the 'Simulink' tool (MATLAB) you can find something which can help you.

  • $\begingroup$ Hi, many thanks for the reply. It's very useful to get these pointers. This is not as straight-forward as I thought. I'm going to consult with my physicist friend about the EKF. (I'm an engineer) $\endgroup$
    – Eamorr
    Sep 12, 2012 at 10:20

As a somewhat tangential word of advice: ditch GPS altitude.

GPS altitude is based on the geoidic model used by the receiver. Not all geoids are the same. GPS altitude estimates coming off receivers are therefore typically very inaccurate -- way too inaccurate for state estimation of an aircraft. In fact, it's so inaccurate that it will corrupt your state estimator quite badly in some cases.

I have analyzed GPS data for ground vehicles, and found that vehicles have made 300 foot drops over traversing 150 feet of distance, according to GPS altitude (obviously, this is false).

As for a reference, I highly recommend Adaptive Control Design and Analysis by Gang Tao. He covers state estimation, and control of state-estimated systems, quite well in this book.

  • $\begingroup$ This is why I love stackexchange. Thanks so much for the tips. $\endgroup$
    – Eamorr
    Sep 12, 2012 at 14:27
  • $\begingroup$ Although I agree that GPS altitude is not very accurate, it is certainly not because of the reason you mention. The GPS geoid is extremely well defined. $\endgroup$
    – fishinear
    Oct 18, 2014 at 14:46
  • $\begingroup$ @fishinear Of course the GPS geoid is extremely well defined. It just happens to not match the actual "rocks and trees are this high" model of the actual earth. $\endgroup$
    – Emily
    Oct 18, 2014 at 14:47

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .