Kalman Filter to determine position and attitude from 6DOF IMU (accelerometer + gyroscope)

Question

I'm going to describe the problem I'm trying to solve and walk through what I understand so far about the Kalman Filter.

I have an IMU which gives me the following measurements every time interval t: accelerations (Ax, Ay, Az), and gyroscope giving angular velocities (pitch, roll, yaw). I want to use sensor fusion (combining the data) to get a very accurate estimate of absolute position (x,y,z) and angular orientation (theta1, theta2, theta3). Research suggests that a Kalman Filter is the way to go. Data fusion of the sensors can help compliment their respective errors: acc is noisy but doesn't drift; gyro is less noisy but does drift.

The model for this system would be something along the lines of:

$x_t = F_t . x_t-1 + w_t$ (no control inputs)

$z_t = H_t . x_t + v_t$

Where,

$x_t$: State at time t; holds the variables (x,y,z,theta1,theta2,theta3)

$F_t$: state transition matrix (representing differential equations, or time-difference equations in this discrete case)

$H_t$: measurement matrix, turns the state we calculate into what we (should) observe as output

$w_t$: estimate (white) noise, with covariance matrix Q_t

$v_t$: measurement (white) noise, with covariance matrix R_t

The Kalman Filter, which has a lovely derivation here, would then consist of two phases I would want to calculate at every time step: (at this stage, my state variable is an estimate)

Predict:

$x_t|t-1 = F_t . x_t-1|t-1$

$P_t|t-1 = F_t . P_t-1|t-1 . F_tT + Q_t$

Where P is the state variance matrix, = covariance($x_t - x_t|t)$

Update:

$x_t|t = x_t|t-1 + K_t . y_t$, and $y_t = z_t - H_t . x_t|t-1$

$K_t = P_t|t-1 . H_tT . (H_t . P_t|t-1 . H_tT + R_t)-1$

$P_t|t = (I - K_t . H_t) . P_t|t-1$

Where $z_t$ is measured output from the sensors, $K_t$ is the Kalman Gain, $R_t$ is measurement covariance, $I$ is the identity matrix. This much makes sense from the Kalman Filter theory I've learned.

Putting it together:

I need to identify the matrices, namely F, H, P, Q, R. This is where I get stuck. H seems to be about the dynamics of the system: transform the accelerations and angular velocities into position and angle. What would that matrix look like? Then there's F, updating the previous state to the next state. I'm not sure how to do that. From what I understand, P is based on initial conditions of how much you "trust" your initial starting position estimate, i.e. $P_0|0$ = diag(large numbers) if you don't "trust" it.

Not sure about Q and R either. R is the measurement noise, meaning I could probably sit the IMU in a stationary position, see how the numbers jump about (noise), and come up with something yeah? I'm not sure what's a good process for doing that. Finally Q, the "error due to process" - is this like the error that my F matrix produces? How would one measure this?

In summary, I am stuck on the dynamic model, and identifying the matrices, particularly Q and R. I'd be happy to be linked to sources that detail the solution to this or something very similar. Thanks!

Answer 1

Let's start by stripping out the Kalman filter terms, because this seems to be where you're initially confused.

Your dynamical system looks like this:

$$\begin{align*} x_t &= F x_{t-1} + w_t, \\ z_t &= H x_t + v_t. \end{align*} $$

I've dropped the subscript $t$ from the matrices since we'll assume for now that those aren't going to change with time.

Your system dynamics are described by $F$. This is sometimes called the "plant matrix". This describes how your states evolve in terms of a previous state vector. So this matrix will completely describe your system dynamics.

The matrix $H$ is your observability matrix. You cannot necessarily directly observe every state. If you can, this term would be the identity matrix. But sometimes what you can observe is a sum of other states. For instance, you might not be able to directly measure the RPMs of your transmission output shaft, but you can measure the RPMs of your engine and your wheels. In this case, your tire RPM is your engine RPM stepped down (or up) by your transmission gear ratio. So while you cannot observe it directly, you can certainly write it as a function of other measurable states.

You should know your $F$ and $H$ matrices, in general, from first principles. In practice, you don't always know them, and you need to estimate them. But that's a different problem not entirely related to Kalman filtering, so for now I'll assume you can derive them.

Kalman filtering comes in because of the noise terms, $w_t$ and $v_t$. There are unmeasurable process noises -- turbulence, manufacturing flaws, etc. -- which are described in $w_t$, and there are sensor noises -- line noise, sensor manufacturing flaws, etc. -- which are described by $v_t$.

The noise in each sensor state is not necessarily independent. For example, if you're flying an airplane, turbulence that affects your left wing will affect every state in your vehicle dynamics. So the effect of process noise is not independent with respect to each state.

Your $R$ and $Q$ matrices describe the interaction between these uncertainty effects. You can estimate them using any of several techniques. Which technique works best is really dependent on your application and your data. Certainly, if you have lots of real-world measurements, you can do some autocovariance computations and approximate these matrices.

Finally, your matrix $P$ is what the Kalman filter is really all about. $P$ is a measure of the following quantity: "given some observed data $z_{t-1}$, where do we expect our states to be." Recall that the fundamental problem is that not all states are directly observable! And -- even when they are, there is noise. But, we have some pretty good faith in the accuracy of what we can observe. So given what we know about the system dynamics and what we observe, can we make a prediction about the state of the system.

The initial estimate of $P$ describes how well we really understand our initial conditions. Sometimes, we can pin them down exactly (for instance, measuring something on the ground). Other times, we're left with imperfect data (for instance, using radar data to estimate where a satellite is).