13 DOF Kalman filter

Question

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

Inputs:

• 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]

Outputs:

• 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,

Answer 1

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.

Answer 2

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.

Answer 3

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.