Extended Kalman Filter for 6-Axis IMU

IMU’s are an increasingly popular way to gather movement data as wearable technology improves. However, the accelerometers and gyroscopes within them are noisy, making signal filters necessary to decrease error.

While there are multiple types of IMUs, one very common type is called a 6-axis IMU. These devices combine a 3-axis accelerometer and a 3-axis gyroscope, and are very handy for collecting data about movement. You can stick them on a shoe, strap them to a leg, or embed them in a wearable device to track how someone moves. Accelerometers measure linear acceleration and can estimate how far someone moves or how fast, but they’re noisy and can be thrown off by quick jolts or vibrations. Gyroscopes, which measure rotational velocity, are great for capturing smooth and precise turning motions, like the angle of a foot during a step. However, they tend to drift over time, leading to small errors that snowball into big ones.

This is where sensor fusion has a lot to offer. By combining data from both sensors, fusion algorithms can balance the strengths of each to help compensate for the weaknesses of the other. Gyroscope data can stabilize noisy accelerometer readings, and accelerometer data can reduce gyroscope drift. The sensor fusion algorithms discussed in this paper help this combination work smoothly, producing clean, reliable data about how someone walks.

Benefits of accurate gait analysis include helping to predict falls in older adults, tracking how well a patient is recovering from surgery, or even fine-tuning athletic performance. Without sensor fusion, IMU data would be too messy or unreliable to perform these functions. Continue for a deep dive into how sensor fusion works in the context of the 6-axis IMU.

Kalman Filter

The Kalman Filter is a recursive algorithm used to estimate the state of dynamic systems from noisy measurements. One of its application is tracking the orientation of an Inertial Measurement Unit (IMU). IMUs provide data on acceleration and angular velocity but are prone to noise and drift over time. The Kalman Filter helps fuse this noisy sensor data with a predictive model to more accurately estimate the IMU’s orientation given the measurement errors. In general, the Kalman Filter’s ability to combine system predictions with real-time observations to produce optimal state estimates makes it useful in dynamic systems where the state changes over time. It is also computationally efficient due to its recursive nature, making it suitable for real-time applications. Here, I describe an EKF (extended kalman filter — one of many types of kalman filters) for a 6-axis IMU.

Kalman Equations

The kalman filter estimates an nx1 column vector state variable ($\mathbf x$), based on some mx1 column vector measurement ($\mathbf z$), using a system model:

After the system model has been set, there are five steps of the simple kalman filter:

0. Set initial values

   $$\begin{align*} \mathbf x_0 &= \text{initial state} &(& \text{nx1 column vector}), \\\\ \mathbf P_0 &= \text{initial error covariance} &(& \text{nxn matrix}). \end{align*}$$

1. Predict state and error covariance:

   $$\begin{align*} \mathbf{\bar x_k} &= \mathbf A \mathbf x_{k-1}, \\\\ \mathbf{\bar P_k} &= \mathbf A \mathbf P_{k-1} \mathbf A^T + \mathbf Q. \end{align*}$$

2. Compute kalman gain:

   $$\mathbf K_k = \mathbf{\bar P}_k \mathbf H^T \left(\mathbf H \mathbf{\bar P}_k \mathbf H^T + \mathbf R\right)^{-1}.$$

3. Compute the estimate (state update equation):

   $$\mathbf x_k = \mathbf{\bar x}_k + \mathbf K_k \left(\mathbf z_k - \mathbf H \mathbf{\bar x}_k\right).$$

4. Compute the error covariance:

   $$\mathbf P_k = \mathbf{\bar P}_k - \mathbf K_k \mathbf H \mathbf{\bar P}_k.$$

Steps 1-4 are then repeated to recursively update with each new $\mathbf z_k$.

Notes:

1. Here, the bar notations denote predicted values before measurement.

2. The term $(\mathbf z_k - \mathbf H \mathbf{\bar x}_k)$ in the state update equation is important, because it represents the gap between our prediction, and our measurement. Because of this importance, it is given the name “measurement residual” or “innovation”.

This can be applied to both uni-variate and multi-variate systems, and the notation is unfortunates not always consistent. Below is an explanation from Roger Labbe in his book Kalman and Bayesian Filters in Python (note that Labbe refers to the state transition as $\mathbf F$ rather than $\mathbf A$):

“[the univariate and multivariate equations]… are quite similar.

Predict

$\begin{array}{|l|l|l|}\hline\text{Univariate} & \text{Univariate} & \text{Multivariate}\\\\& \text{(Kalman form)} & \\\\\hline\bar \mu = \mu + \mu_{f_x} & \bar x = x + dx & \bar{\mathbf x} = \mathbf{Fx} + \mathbf{Bu}\\\\\bar\sigma^2 = \sigma_x^2 + \sigma_{f_x}^2 & \bar P = P + Q & \bar{\mathbf P} = \mathbf{FPF}^\mathsf T + \mathbf Q >\\\\\hline\end{array}$

Without worrying about the specifics of the linear algebra, we can see that: $\mathbf x,\, \mathbf P$ are the state mean and covariance. They correspond to $x$ and $\sigma^2$.$\mathbf F$ is the state transition function. When multiplied by $\bf x$ it computes the prior. $\mathbf Q$ is the process covariance. It corresponds to $\sigma^2_{f_x}$. $\mathbf B$ and $\mathbf u$ are new to us. They let us model control inputs to the system.

Update

$\begin{array}{|l|l|l|}\hline\text{Univariate} & \text{Univariate} & \text{Multivariate}\\\\& \text{(Kalman form)} & \\\\\hline& y = z - \bar x & \mathbf y = \mathbf z - \mathbf{H\bar x} \\\\& K = \frac{\bar P}{\bar P+R}&\mathbf K = \mathbf{\bar{P}H}^\mathsf T (\mathbf{H\bar{P}H}^\mathsf T + \mathbf R)^{-1} \\\\\mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} & x = \bar x + Ky & \mathbf x = \bar{\mathbf x} + \mathbf{Ky} \\\\\sigma^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} & P = (1-K)\bar P & \mathbf P = (\mathbf I -\mathbf{KH})\mathbf{\bar{P}} \\\\\hline\end{array}$

$\mathbf H$ is the measurement function. We haven’t seen this yet in this book and I’ll explain it later. If you mentally remove $\mathbf H$ from the equations, you should be able to see these equations are similar as well.

$\mathbf z, \mathbf R$ are the measurement mean and noise covariance. They correspond to $z$ and $\sigma_z^2$ in the univariate filter (I’ve substituted $\mu$ with $x$ for the univariate equations to make the notation as similar as possible).

$\mathbf y$ and $\mathbf K$ are the residual and Kalman gain.

The details will be different than the univariate filter because these are vectors and matrices, but the concepts are exactly the same:

• Use a Gaussian to represent our estimate of the state and error
• Use a Gaussian to represent the measurement and its error
• Use a Gaussian to represent the process model
• Use the process model to predict the next state (the prior)
• Form an estimate part way between the measurement and the prior Your job as a designer will be to design the state $\left(\mathbf x, \mathbf P\right)$, the process $\left(\mathbf F, \mathbf Q\right)$, the measurement $\left(\mathbf z, \mathbf R\right)$, and the measurement function $\mathbf H$. If the system has control inputs, such as a robot, you will also design $\mathbf B$ and $\mathbf u$.”

Lets try applying this to our problem. In the following sections, I describe the math and the python code I used to implement the filter.

Python Info

import numpy as np

# Define the matrix
matrix = np.array([[6, 2, 4],
                   [-1, 4, 3],
                   [-2, 9, 3]])

# Define the vector
vector = np.array([[4], [-2], [1]])

# Perform the matrix-vector multiplication
result = matrix @ vector  # Alternatively, use np.dot(matrix, vector)

print(result)

# Predict State and Error Covariance
xp = A @ x
Pp = A @ P @ A.T + Q

# Compute Kalman Gain
K = Pp @ H.T @ numpy.linalg.inv(H @ Pp @ H.T + R)

# State Update
x = xp + K @ (z - H @ xp)

# Compute Error Covariance
P = Pp - (K @ H @ Pp)

Defining the Measurement Variable

When using an IMU for gait analysis, we would like to use the IMU’s measurements to calculate heel-strike, toe-off, and stride length (and perhaps we’ll add toe-down and heel-off if we’re feeling ambitious). At any given time $k$, the IMU will give us accelerometer data along its three local axes. We can think of this accereration data as a vector $\mathbf a^\text{local}$, where at time $k$, we have

It will also give us rotational velocity along these local axes which we can write as

Putting these together, we can think of our measurements as being represented by a variable $\mathbf z$, where at time $k$ the IMU gives us the reading

It’s important to keep in mind that these measurements are with respect to the local frame of the IMU, and not the world frame.

Defining the State Variable

In order to determine when and how gait events happen, we would need to know the IMU’s position and orientation in world frame axes, such as north($N$)-east($E$)-down($D$) axes. Additionally, it would be nice to have the IMU’s velocity and acceleration in the world frame. To visualize this, we could assign variables to position, linear velocity, linear acceleration, orientation, and angular velocity, like this:

Here, $\mathbf q_k^\text{world}$ is a vector representation of the quaternion $\left[q^0_k + i\left(q^1_k\right) + j\left(q^2_k\right) + k\left(q^3_k\right)\right]$. I use quaternions rather than matricies to represent orientation because they let us update our orientation using the quaternion update function

providing a consistent method for interpolating angles between time steps ($\otimes$ represents the “Hamilton product”, AKA quaternion multiplication).

Putting these together, we can think of our system state (at least the parts we care about) as being represented by a variable $\mathbf x$, where at time $k$ we estimate that its properties are

Quaternion Info

Translating Between Local and World Frames

Our goal is to use our local pitch-roll-yaw coordinate system measurements to estimate the system state in terms of the global coordinate system. A difficulty with calculating acceleration in this manner is that the direction of gravity will change as our local axes rotate, and our accelerometers will not be able to distinguish this change in gravity from a change in linear acceleration. In this section, we use our quaternion orientation to address the issue.

At a given time $k$, we will have the IMU’s orientation stored as a quaterion $\mathbf q_k$ which represents the rotation from a “neutral” orientation to IMU’s current orientation. In order to calculate the “down” direction from this, it is most efficient to convert this quaternion to a matrix. The rotation matrix $\mathbf C_k$, defined as

rotates a vector from the local frame to the world frame. In other words, we have

Furthermore, since $\mathbf C_k$ is an orthogonal matrix, its inverse must be equal to its transpose $\mathbf C^T_k$, and thus

Because of this, we can calculate world frame acceleration from our local measurements in a way that accounts for gravity. If we are measuring acceleration in m/s^2 units then we will always have

Therefore, a stationary sensor with any given pitch-roll-yaw axes should read

By extension, any deviation from this value means that the sensor is accelerating in the world frame, so at any time $k$ our sensor should read

This looks like exactly what we need! To make things more concise, we will add $\mathbf a^{\text{world}}$ and gravity together into one vector, and write

Now that we’ve defined our problem and seen a bit of how our measurment and state variables relate to each other, it’s time to build a sensor fusion algorith to estimate state from measurements.

def c_matrix(quaternion: np.ndarray) -> np.ndarray:
    if len(quaternion) != 4:
        raise ValueError(f"Expected quaternion of length 4, got {len(quaternion)} instead")

    quaternion = quaternion.reshape(-1, 1)
    q0 = quaternion[0, 0]
    q1 = quaternion[1, 0]
    q2 = quaternion[2, 0]
    q3 = quaternion[3, 0]

    c = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
                  [2*(q1*q2+q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)],
                  [2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1**2+q2**2)]])

    return c

quat = x[9:13]
print(c_matrix(quat))

State

The state of the kalman filter is described by state variable $\mathbf x$ and the covariance $\mathbf P$. In this section, we will discuss how to set their initial values. After we set their initial values, our kalman filter will update them internally at each time step.

x

As described in the “State Variable” section, we want $\mathbf x$ to be a be a 16x1 vector. If we could set our origin at the initial position of the IMU, and we could be fairly certain that it would be stationary and aligned with $N$-$E$-$D$ axes when we start recording data, then a resonable initial state $\mathbf x_0$ might look like:

x = np.array([[0], [0], [0], # position
              [0], [0], [0], # velocity
              [0], [0], [0], # acceleration
              [1], [0], [0], [0], # orientation quaternion
              [0], [0], [0]]) # rotational velocity

P

The state covariance $\mathbf P$ will be a 16x16 (or 13x13) matrix which represents the covariance of the state. A reasonable $\mathbf P_0$ would be:

where $\sigma^2_{p^N_0}$ is the variance in the initial position in the north direction, and so on and so forth. As a general rule of thumb, it will be better to overestimate than underestimate — the filter will converge if $\mathbf P_0$ is too large, but might not if it’s too small.

Process

The process of the kalman filter is described by $\mathbf F$ (the state transition function) and $\mathbf Q$ (the process covariance).

F

Since we do not have any predetermined control inputs here, we want a 16x16 matrix $\mathbf F$ which we can multiply by the current state to get our predicted state in the next time step. To visualize this, we want a matrix which satisfies

There are a few things we need to make this happen.

1. Position Update

Since the time between measurements is $dt$, then we want it to update the postion in a way that satisfies

where $dt$ is the time step between measurements. This expands to

Therefore, the top three rows of our matrix will be

2. Velocity Update

We also want it to update the velocity in way that satisfies

which similarly expands to

and similarly gives us the next three rows of our matrix

3. Acceleration Update

To keep things simple, we won’t predict change here, so we’ll use

Therefore, next three rows of our matrix will be

4. Orientation Update

We want it to update the orientation in a way that satisfies the quaternion update function

satisfy our rotation update equation.Let’s start by expanding our quaternion multiplication term.

We know that the product of two quaternions

and

is calculated using the formula:

Substituting

gives

Writing this result in vector form, we have