# Quaternion-Based EKF for Attitude Estimation

This page describes a method to estimate orientation given gyroscope and accelerometer data. Extended Kalman filter (EKF) is used with quaternion as state vector. A working Python code is also provided.

## Overview

Given system and measurement equations,

Discrete-time EKF steps are,

 1 Prediction: state 2 Prediction: covariance 3 Innovation 4 Innovation: covariance 5 Kalman gain 6 Update: state 7 Update: state

where

### Gyroscope

Gyroscope data are assumed to be angular velocity in rad/sec in the body frame:

Gyroscope data are not treated as observations. Instead, they are used in the prediction step. This is done because modeling the system dynamics accurately is difficult [1].

### Accelerometer

Accelerometer data are assumed to be in m/s^2 in the body frame:

Accelerometer data are treated as observations. The device acceleration is assumed to be small enough so that the accelerometer vector always points up relative to the ground.

### Coordinates

Front-right-down for body frame (sensor frame). NED for world frame. For example, accelerometer measures

when the device is not rotated (when body frame matches world frame).

### Notation

⨂ denotes quaternion multiplication defined by:

Not to be confused with some papers [2] that define the product in reverse order.

## State, x

State vector is quaternion:

It is a unit quaternion and represents the orientation of the device. It is normalized every time it is modified. The four components of the quaternion is treated as independent parameters.

## State Prediction, f(x)

The state is modified with gyroscope data. Below are two ways to do this.

### Method 1

First, calculate the rotation vector for the current time step

Next, convert the rotation vector to axis angle representation:

Then, convert it to quaternion:

The state is predicted as follows:

We are post-multiplying because qr is done relative to the body frame.

### Method 2

Use the below approximation to update the state:

where the quaternion derivative, qdot, is given by:

where

## State Transition Matrix, F

State transition matrix F is needed to predict the covariance P. F is a 4x4 matrix given by:

where

Three ways to derive F are provided:

### Derivation 1

Axis-angle representation for a rotation (θ, û) is equivalent to the unit quaternion representation:

An infinitesimal rotation is then

Rotation vector for the current time step is given by

then,

The discretized dynamics is then

The jacobian of the above is given by

### Derivation 2

Quaternion derivative is given by

which can be discretized as

using the matrix exponential formula:

The above can be approximated by only taking the first two terms as,

The jacobian of the above is identical to that from drivation 1.

### Derivation 3

Quaternion derivative can be approximated as

which can be rearranged as:

The jacobian of the above is identical to that from derivation 1 and 2.

## Process Noise

Gyroscope noise appears as state noise and not observation noise. Process noise is given by,

where

σgx, σgy, σgz are gyro noise in rad/sec.

### Derivation of W

From the previous section, we know that

So the jacobian is:

## Observation, z

Observation vector is the gravity vector in body frame in m/s^2:

where vector a is accelerometer measurement. Only the accelerometer measurement vector direction is used.

## Observation model, h(x)

Observation model maps the state space into the observed space:

where Rq is the rotation matrix representing the orientation. The above converts the gravity vector in world frame to body frame. Quaternion to rotation matrix is given by

Not to be confused with some papers [2] that define the rotation matrix as the transpose of the above.

## Observation model, H

Observation matrix is determined by taking the jacobian of the observation function:

## Observation noise

Accelerometer noise appears as observation noise. Observation noise is given by,

σax, σay, σaz are accelerometer noise in m/s^2

## Python Implementation

ekf.py

```import numpy as np

g = 9.80665

def normalize(v):
return np.array(v) / np.linalg.norm(v)

def quaternion_to_matrix(q):
w, x, y, z = q
w2 = w * w
x2 = x * x
y2 = y * y
z2 = z * z
r11 = w2 + x2 - y2 - z2
r12 = 2 * (x * y - w * z)
r13 = 2 * (x * z + w * y)
r21 = 2 * (x * y + w * z)
r22 = w2 - x2 + y2 - z2
r23 = 2 * (y * z - w * x)
r31 = 2 * (x * z - w * y)
r32 = 2 * (y * z + w * x)
r33 = w2 - x2 - y2 + z2
return np.array([
[r11, r12, r13],
[r21, r22, r23],
[r31, r32, r33]
])

def quaternion_multiply(p, q):
r0 = p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3]
r1 = p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2]
r2 = p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1]
r3 = p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]
return [r0, r1, r2, r3]

q1 = unit_axis[0] * sin_half
q2 = unit_axis[1] * sin_half
q3 = unit_axis[2] * sin_half
return [q0, q1, q2, q3]

def quaternion_from_rotation_vector(v, eps=0):
angle_rad = np.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])

# Guard against division by zero
else:
q_unit = [1, 0, 0, 0]

return q_unit

def get_F(w, dt):
wx, wy, wz = w
Omega = 1/2 * np.array([
[ 0, -wx, -wy, -wz],
[wx,   0,  wz, -wy],
[wy, -wz,   0,  wx],
[wz,  wy, -wx,   0]
])
return np.identity(4) + Omega * dt

def get_W(q, dt):
qw, qx, qy, qz = q
return dt/2 * np.array([
[-qx, -qy, -qz],
[ qw, -qz,  qy],
[ qz,  qw, -qx],
[-qy,  qx,  qw]
])

def get_H(q):
qw, qx, qy, qz = q
return 2 * g * np.array([
[ qy, -qz,  qw, -qx],
[-qx, -qw, -qz, -qy],
[-qw,  qx,  qy, -qz]
])

def f(x, w, dt):
d_ang = np.array(w) * dt
dq = quaternion_from_rotation_vector(d_ang)
x = quaternion_multiply(x, dq)
x = normalize(x)
return x

def h(x):
R_from_body = quaternion_to_matrix(x)
R_to_body = R_from_body.T
return R_to_body @ np.array([[0], [0], [-g]])

class EKF:
def __init__(self, q0=[1,0,0,0], gyro_noise=0.015, accelerometer_noise=1.0):
'''
q0 -- initial orientation (unit quaternion) [qw, qx, qy, qz]
accelerometer_noise -- [m/s^2]
'''
self.x = q0
self.P = np.identity(4) * 0.01
self.Q = np.identity(3) * (gyro_noise ** 2)
self.R = np.identity(3) * (accelerometer_noise ** 2)

def predict(self, w, dt):
'''
w -- gyro measurement in front-right-down coordinates [rad/sec]
dt -- time step [sec]
'''
F = get_F(w, dt)
W = get_W(self.x, dt)
self.x = f(self.x, w, dt)
self.P = F @ self.P @ F.T + W @ self.Q @ W.T

def update(self, a):
'''
a -- accelerometer measurement in front-right-down coordinates [m/s^2]
'''
a = np.array(a).reshape(3, 1)
z = g * normalize(a)
y = z - h(self.x)
H = get_H(self.x)
S = H @ self.P @ H.T + self.R
K = (self.P @ H.T) @ np.linalg.inv(S)
self.x = self.x + (K @ y)[:,0]
self.x = normalize(self.x)
self.P = (np.identity(4) - K @ H) @ self.P
```

## Simulation Test

True vs estimated attitudes are compared using simulated sensor data. Euler angles are z-y-x intrinsic Tait-Bryan rotation (e.g. first rotate about Z-axis by angle yaw, next rotate about the newly created Y-axis by angle pitch, finally rotate about the newly created X-axis by angle roll).

### Test 1: static case

IMU is held fixed (non-rotating) at roll 25 degrees, pitch 0 degrees, yaw 0 degrees. Since the initial orientation is roll 0 degrees, pitch 0 degrees, yaw 0 degrees, it takes about 1 second for the estimate to stabilize to the true value.

### Test 2: constant angular rate

IMU is rotated around the x-axis at constant angular rate of 90 degrees/sec. It can be seen that the roll is estimated correctly even after going full rotation. Pitch and yaw (which are kept constant) is accurate to about +/-1 degree. Pitch is inaccurate to about 2.5 degrees at the beginning most likely because the state covariance has not reached steady state yet.

### Test 3: rotating all axes

IMU is rotated all axes at the same time. It can be seen that the estimated orientation closely matches the true value.

### Source code for the above tests

It is assumed that ekf.py is placed in the same folder as the below script.

```import matplotlib.pyplot as plt
import numpy as np
from ekf import EKF
from ekf import quaternion_multiply
from ekf import quaternion_from_axis_angle
from ekf import normalize
from ekf import g

random_generator = np.random.default_rng(0)

def quaternion_identity():
return [1, 0, 0, 0]

def quaternion_inv(q_unit):
return [q_unit[0], -q_unit[1], -q_unit[2], -q_unit[3]]

def rotate_vector(q_unit, v):
p = [0.0, v[0], v[1], v[2]]
# q * p * q^-1
r = quaternion_multiply(
quaternion_multiply(q_unit, p),
quaternion_inv(q_unit))
return [r[1], r[2], r[3]]

def quaternion_to_euler(q):
roll  = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]),
1 - 2 * (q[1] * q[1] + q[2] * q[2]))
pitch = np.arcsin(2 * (q[0] * q[2] - q[3] * q[1]))
yaw   = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]),
1 - 2 * (q[2] * q[2] + q[3] * q[3]))
return roll, pitch, yaw

def quaternion_from_euler(roll, pitch, yaw):
c_roll   = np.cos(roll / 2)
s_roll   = np.sin(roll / 2)
c_pitch = np.cos(pitch / 2)
s_pitch = np.sin(pitch / 2)
c_yaw   = np.cos(yaw / 2)
s_yaw   = np.sin(yaw / 2)

q0 = c_roll * c_pitch * c_yaw + s_roll * s_pitch * s_yaw
q1 = s_roll * c_pitch * c_yaw - c_roll * s_pitch * s_yaw
q2 = c_roll * s_pitch * c_yaw + s_roll * c_pitch * s_yaw
q3 = c_roll * c_pitch * s_yaw - s_roll * s_pitch * c_yaw
return [q0, q1, q2, q3]

def plot(ts, w, a, rpy, estimated_rpy):
fig, axs = plt.subplots(5, 1)

axs[0].plot(ts, w[0], label='x')
axs[0].plot(ts, w[1], label='y')
axs[0].plot(ts, w[2], label='z')
axs[0].grid(True)
axs[0].legend(loc='upper right')
axs[0].set_xticklabels([])

axs[1].plot(ts, a[0], label='x')
axs[1].plot(ts, a[1], label='y')
axs[1].plot(ts, a[2], label='z')
axs[1].grid(True)
axs[1].legend(loc='upper right')
axs[1].set_ylabel('a (m/s^2)')
axs[1].set_xticklabels([])

axs[2].plot(ts, rpy[0], label='true')
axs[2].plot(ts, estimated_rpy[0], '--', label=f'estimated')
axs[2].grid(True)
axs[2].legend(loc='upper right')
axs[2].set_ylabel('roll (deg)')
axs[2].set_xticklabels([])

axs[3].plot(ts, rpy[1], label='true')
axs[3].plot(ts, estimated_rpy[1], '--', label='estimated')
axs[3].grid(True)
axs[3].legend(loc='upper right')
axs[3].set_ylabel('pitch (deg)')
axs[3].set_xticklabels([])

axs[4].plot(ts, rpy[2], label='true')
axs[4].plot(ts, estimated_rpy[2], '--', label='estimated')
axs[4].grid(True)
axs[4].legend(loc='upper right')
axs[4].set_ylabel('yaw (deg)')
axs[4].set_xlabel('time (sec)')

plt.show()

def get_gyro_data(q1, q2, dt, noise):
mean_x = 2/dt * (q1[0]*q2[1] - q1[1]*q2[0] - q1[2]*q2[3] + q1[3]*q2[2])
mean_y = 2/dt * (q1[0]*q2[2] + q1[1]*q2[3] - q1[2]*q2[0] - q1[3]*q2[1])
mean_z = 2/dt * (q1[0]*q2[3] - q1[1]*q2[2] + q1[2]*q2[1] - q1[3]*q2[0])
x = random_generator.normal(mean_x, noise)
y = random_generator.normal(mean_y, noise)
z = random_generator.normal(mean_z, noise)
return x, y, z

def get_accelerometer_data(q, noise):
q_to_body = quaternion_inv(q)
up_world = [0, 0, -g]
up_body = rotate_vector(q_to_body, up_world)
x = random_generator.normal(up_body[0], noise)
y = random_generator.normal(up_body[1], noise)
z = random_generator.normal(up_body[2], noise)
return x, y, z

def generate_accelerometer_data_array(q, acc_noise):
N = len(q)
ax, ay, az = [None] * N, [None] * N, [None] * N

for i in range(N):
ax[i], ay[i], az[i] = get_accelerometer_data(q[i], acc_noise)

return ax, ay, az

def generate_gyro_data_array(t, q, gyro_noise):
N = len(t)
wx, wy, wz = [None] * N, [None] * N, [None] * N

for i in range(1, N):
dt = t[i] - t[i-1]
wx[i], wy[i], wz[i] = get_gyro_data(q[i-1], q[i], dt, gyro_noise)

return wx, wy, wz

def estimate(t, w, a):
N = len(t)
q = [None] * N
ekf = EKF()

for i in range(1, N):
dt = t[i] - t[i-1]
wi = [w[0][i], w[1][i], w[2][i]]
ai = [a[0][i], a[1][i], a[2][i]]
ekf.predict(wi, dt)
ekf.update(ai)
q[i] = ekf.x

return q

def quaternion_to_euler_degrees(q):
N = len(q)
roll = [None] * N
pitch = [None] * N
yaw = [None] * N

for i in range(N):
if q[i] is not None:
r, p, y = quaternion_to_euler(q[i])
roll[i] = np.degrees(r)
pitch[i] = np.degrees(p)
yaw[i] = np.degrees(y)

return roll, pitch, yaw

# Fixed (non-rotating) at roll 25 degrees, pitch 0 degrees, yaw 0 degrees
def generate_test1_data(t):
N = len(t)
q = [None] * N

for i in range(N):
q[i] = quaternion_from_euler(roll, pitch, yaw)

return q

# Rotating at constant angular rate [90 degrees/sec, 0, 0]
def generate_test2_data(t):
N = len(t)
q = [None] * N
q[0] = quaternion_identity()

rate = 90
abs_w = np.linalg.norm(w)

for i in range(1, N):
dt = t[i] - t[i-1]

dtheta = abs_w * dt
unit_axis = w / abs_w
dq = quaternion_from_axis_angle(unit_axis, dtheta)

q[i] = quaternion_multiply(q[i-1], dq)
q[i] = normalize(q[i])

return q

# Rotating all axes at the same time
def generate_test3_data(t):
N = len(t)
q = [None] * N
q[0] = quaternion_identity()

wx = 500 * np.sin(2 * np.pi * 0.5 * t)
wy = 200 * np.sin(2 * np.pi * 1.0 * t)
wz = 300 * np.sin(2 * np.pi * 2.0 * t)

for i in range(1, N):
dt = t[i] - t[i-1]

abs_w = np.linalg.norm(w)

if abs_w > 0:
dtheta = abs_w * dt
unit_axis = w / abs_w
dq = quaternion_from_axis_angle(unit_axis, dtheta)
else:
dq = quaternion_identity()

dq = quaternion_from_axis_angle(unit_axis, dtheta)
q[i] = quaternion_multiply(q[i-1], dq)
q[i] = normalize(q[i])

return q

ACCELEROMETER_NOISE = 1.0 # m/s^2

# Generate time
t0 = 0
tf = 5
dt = 0.01
t = np.arange(t0, tf, dt)

# Generate test data
#q = generate_test1_data(t)
#q = generate_test2_data(t)
q = generate_test3_data(t)

# Generate sensor data
a = generate_accelerometer_data_array(q, ACCELEROMETER_NOISE)
w = generate_gyro_data_array(t, q, GYRO_NOISE)

# EKF estimation
estimated_q = estimate(t, w, a)

# Check results
rpy = quaternion_to_euler_degrees(q)
estimated_rpy = quaternion_to_euler_degrees(estimated_q)
plot(t, w, a, rpy, estimated_rpy)
```

## References

[1] Lefferts, E.J., Markley, F.L., and Shuster, M.D. "Kalman filtering for spacecraft attitude estimation" 1982
[2] Bar-Itzhack, I. Y., and Oshman, Y., "Attitude Determination from Vector Observations: Quaternion Estimation" 1985