This page describes a method to estimate orientation given gyroscope and accelerometer data. Extended Kalman filter (EKF) is used with quaternion and gyro bias as state vector. A working Python code is also provided.
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 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]. The measured angular velocity has bias applied to the true angular velocity:
Accelerometer data are assumed to be in m/s2 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.
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).
⨂ denotes quaternion multiplication defined by:
Not to be confused with some papers [2] that define the product in reverse order.
1g is 9.80665m/s2
State vector is:
The first four parameters are components of a unit quaternion and represents the orientation of the device. It is normalized every time it is modified. The last three parameters are gyro bias vector in rad/sec.
Gyro bias vector stays the same in the prediction step. Orientation (quaternion) is modified with gyroscope data as follows:
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 Δq is done relative to the body frame.
State transition matrix F is needed to predict the covariance P. F is a 7x7 matrix given by:
F is derived as follows:
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
Bias does not change so the full state transition is given by,
Solving the jacobian of the above with respect to the state vector results in the state transition matrix shown at the beginning of this section.
Gyroscope noise appears as state noise and not observation noise. Process noise is a combination of gyro noise and gyro bias drift and is given by,
where
σgx, σgy, σgz are gyro noise in rad/sec.
σbx, σby, σbz are gyro bias noise in rad/sec.
From the previous section, we know that
Solving the jacobian of the above with respect to the angular velocity results in W matrix shown at the beginning of this section.
Observation vector is the gravity vector in body frame in m/s2:
where g = 9.80665m/s^2 and \vec{a} is accelerometer measurement. Only the accelerometer measurement vector direction is used.
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 matrix is determined by taking the jacobian of the observation function:
Accelerometer noise appears as observation noise. Observation noise is given by,
where
σax, σay, σaz are accelerometer noise in m/s^2
From the previous section, we know that
so
As a result, observation noise simplifies to R.
filter.py
import numpy as np g = 9.80665 def normalize(v): return v / np.linalg.norm(v) def quaternion_identity(): return np.c_[[1, 0, 0, 0]] def quaternion_to_matrix(q_unit): w, x, y, z = q_unit.flat 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 * (w * y + x * z) 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): pw, px, py, pz = p.flat qw, qx, qy, qz = q.flat rw = pw * qw - px * qx - py * qy - pz * qz rx = pw * qx + px * qw + py * qz - pz * qy ry = pw * qy - px * qz + py * qw + pz * qx rz = pw * qz + px * qy - py * qx + pz * qw return np.c_[[rw, rx, ry, rz]] def quaternion_from_axis_angle(unit_axis, angle_rad): ux, uy, uz = unit_axis.flat sin_half = np.sin(angle_rad / 2) qw = np.cos(angle_rad / 2) qx = ux * sin_half qy = uy * sin_half qz = uz * sin_half return np.c_[[qw, qx, qy, qz]] def quaternion_from_rotation_vector(v, eps=0): angle_rad = np.linalg.norm(v) # Guard against division by zero if angle_rad > eps: unit_axis = v / angle_rad q_unit = quaternion_from_axis_angle(unit_axis, angle_rad) else: q_unit = quaternion_identity() return q_unit def get_F(x, w, dt): qw, qx, qy, qz, bx, by, bz = x.flat wx, wy, wz = w.flat return np.array([ [ 1, dt*(-wx + bx)/2, dt*(-wy + by)/2, dt*(-wz + bz)/2, dt*qx/2, dt*qy/2, dt*qz/2], [dt*(wx - bx)/2, 1, dt*( wz - bz)/2, dt*(-wy + by)/2, -dt*qw/2, dt*qz/2, -dt*qy/2], [dt*(wy - by)/2, dt*(-wz + bz)/2, 1, dt*( wx - bx)/2, -dt*qz/2, -dt*qw/2, dt*qx/2], [dt*(wz - bz)/2, dt*( wy - by)/2, dt*(-wx + bx)/2, 1, dt*qy/2, -dt*qx/2, -dt*qw/2], [ 0, 0, 0, 0, 1, 0, 0], [ 0, 0, 0, 0, 0, 1, 0], [ 0, 0, 0, 0, 0, 0, 1] ]) def get_W(x, dt): qw, qx, qy, qz = x.flat[0:4] return dt/2 * np.array([ [-qx, -qy, -qz], [ qw, -qz, qy], [ qz, qw, -qx], [-qy, qx, qw], [ 0, 0, 0], [ 0, 0, 0], [ 0, 0, 0] ]) def get_H(x): qw, qx, qy, qz = x.flat[0:4] return 2 * g * np.array([ [ qy, -qz, qw, -qx, 0, 0, 0], [-qx, -qw, -qz, -qy, 0, 0, 0], [-qw, qx, qy, -qz, 0, 0, 0] ]) def f(x, w, dt): q = x[0:4] b = x[4:7] d_ang = (w - b) * dt dq = quaternion_from_rotation_vector(d_ang) q = quaternion_multiply(q, dq) q = normalize(q) return np.r_[q, b] def h(x): q = x[0:4] R_from_body = quaternion_to_matrix(q) return R_from_body.T @ np.c_[[0, 0, -g]] class EKF: def __init__(self, q0=[1,0,0,0], b0=[0,0,0], init_gyro_bias_err=0.1, gyro_noise=0.015, gyro_bias_noise=0.002, accelerometer_noise=1.0): ''' q0 -- initial orientation (unit quaternion) [qw, qx, qy, qz] b0 -- initial gyro bias [rad/sec] [bx, by, bz] init_gyro_bias_err -- initial gyro bias uncertainty (1 standard deviation) [rad/sec] gyro_noise -- Gyro noise (1 standard deviation) [rad/sec] gyro_bias_noise -- Gyro bias noise (1 standard deviation) [rad/sec] accelerometer_noise -- Accelerometer measurement noise (1 standard deviation) [m/s^2] ''' self.x = np.c_[q0 + b0] # State self.P = np.zeros((7, 7)) self.P[0:4, 0:4] = np.identity(4) * 0.01 self.P[4:7, 4:7] = np.identity(3) * (init_gyro_bias_err ** 2) self.Q = np.identity(3) * (gyro_noise ** 2) self.Q_bias = np.zeros((7, 7)) self.Q_bias[4:7, 4:7] = np.identity(3) * (gyro_bias_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 -- [sec] ''' w = np.c_[w] F = get_F(self.x, 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 + self.Q_bias def update(self, a): ''' a -- accelerometer measurement in front-right-down coordinates [m/s^2] ''' a = np.c_[a] 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) self.x[0:4] = normalize(self.x[0:4]) self.P = (np.identity(7) - K @ H) @ self.P
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).
IMU is held fixed (non-rotating) at roll 25 degrees, pitch 0 degrees, yaw 0 degrees with gryo bias in x-direction of 0.1 rad/sec. Gyro bias in x-direction converges to the true value after about 1 second. Orientation (roll) is correctly tracked even with gyro bias.
IMU is rotated around the x-axis at constant angular rate of 90 degrees/sec with gyro bias in x-direction of 0.1 rad/sec. Roll is estimated correctly even with gyro bias.
IMU is rotated all axes at the same time with gyro bias of (0.1, 0.2, -0.1) rad/sec. It can be seen that the estimated orientation closely matches the true value.
It is assumed that filter.py is placed in the same folder as the below script.
import matplotlib.pyplot as plt import numpy as np from filter import EKF from filter import quaternion_identity from filter import quaternion_multiply from filter import quaternion_from_axis_angle from filter import normalize from filter import g random_generator = np.random.default_rng(0) 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) qw = c_roll * c_pitch * c_yaw + s_roll * s_pitch * s_yaw qx = s_roll * c_pitch * c_yaw - c_roll * s_pitch * s_yaw qy = c_roll * s_pitch * c_yaw + s_roll * c_pitch * s_yaw qz = c_roll * c_pitch * s_yaw - s_roll * s_pitch * c_yaw return np.c_[[qw, qx, qy, qz]] def quaternion_to_euler(q): qw, qx, qy, qz = q.flat roll = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy)) pitch = np.arcsin(2 * (qw * qy - qz * qx)) yaw = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) return roll, pitch, yaw def quaternion_inv(q_unit): w, x, y, z = q_unit.flat return np.c_[[w, -x, -y, -z]] def rotate_vector(q_unit, v): p = np.r_[[[0]], v] # q * p * q^-1 r = quaternion_multiply( quaternion_multiply(q_unit, p), quaternion_inv(q_unit)) return r[1:4] def plot(ts, w, a, rpy, estimated_rpy, b, estimated_b, b_err): bx = np.array(estimated_b[0]) by = np.array(estimated_b[1]) bz = np.array(estimated_b[2]) bx_std = np.array(b_err[0]) by_std = np.array(b_err[1]) bz_std = np.array(b_err[2]) fig, axs = plt.subplots(8, 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_ylabel('w\n(rad/s)') 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\n(m/s^2)') axs[1].set_xticklabels([]) axs[2].plot(ts, rpy[0], label='true') axs[2].plot(ts, estimated_rpy[0], '--', label='estimated') axs[2].grid(True) axs[2].legend(loc='upper right') axs[2].set_ylabel('roll\n(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\n(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\n(deg)') axs[4].set_xticklabels([]) axs[5].plot(ts, b[0], label='true') axs[5].plot(ts, bx, '--', label='estimated') axs[5].fill_between(t, bx-bx_std, bx+bx_std, color='gray', alpha=0.5) axs[5].grid(True) axs[5].legend(loc='upper right') axs[5].set_ylabel('bias x\n(rad/sec)') axs[5].set_xticklabels([]) axs[6].plot(ts, b[1], label='true') axs[6].plot(ts, by, '--', label='estimated') axs[6].fill_between(t, by-by_std, by+by_std, color='gray', alpha=0.5) axs[6].grid(True) axs[6].legend(loc='upper right') axs[6].set_ylabel('bias y\n(rad/sec)') axs[6].set_xticklabels([]) axs[7].plot(ts, b[2], label='true') axs[7].plot(ts, bz, '--', label='estimated') axs[7].fill_between(t, bz-bz_std, bz+bz_std, color='gray', alpha=0.5) axs[7].grid(True) axs[7].legend(loc='upper right') axs[7].set_ylabel('bias z\n(rad/sec)') plt.show() def get_gyro_data(q1, q2, bias, dt, noise): q1_w, q1_x, q1_y, q1_z = q1.flat q2_w, q2_x, q2_y, q2_z = q2.flat mean = np.array([ 2/dt * (q1_w*q2_x - q1_x*q2_w - q1_y*q2_z + q1_z*q2_y), 2/dt * (q1_w*q2_y + q1_x*q2_z - q1_y*q2_w - q1_z*q2_x), 2/dt * (q1_w*q2_z - q1_x*q2_y + q1_y*q2_x - q1_z*q2_w) ]) w = random_generator.normal(mean + bias, noise) return w def get_accelerometer_data(q, noise): q_to_body = quaternion_inv(q) up_world = np.c_[[0, 0, -g]] up_body = rotate_vector(q_to_body, up_world) a = random_generator.normal(up_body, noise) return a 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): a = get_accelerometer_data(q[i], acc_noise) ax[i], ay[i], az[i] = a.flat return ax, ay, az def generate_gyro_data_array(t, q, b, 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] bi = np.array([b[0][i], b[1][i], b[2][i]]) w = get_gyro_data(q[i-1], q[i], bi, dt, gyro_noise) wx[i], wy[i], wz[i] = w.flat return wx, wy, wz def estimate(t, w, a): N = len(t) q = [None] * N bx, by, bz = [None] * N, [None] * N, [None] * N bx_err, by_err, bz_err = [None] * N, [None] * N, [None] * N ekf = EKF() q[0] = ekf.x[0:4] bx[0], by[0], bz[0] = ekf.x.flat[4:7] bx_err[0] = np.sqrt(ekf.P[4, 4]) by_err[0] = np.sqrt(ekf.P[5, 5]) bz_err[0] = np.sqrt(ekf.P[6, 6]) 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[0:4] bx[i], by[i], bz[i] = ekf.x.flat[4:7] bx_err[i] = np.sqrt(ekf.P[4, 4]) by_err[i] = np.sqrt(ekf.P[5, 5]) bz_err[i] = np.sqrt(ekf.P[6, 6]) return q, [bx, by, bz], [bx_err, by_err, bz_err] 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 def euler_degrees_to_quaternion(rpy): roll = rpy[0] pitch = rpy[1] yaw = rpy[2] N = len(roll) q = [None] * N for i in range(N): if roll[i] is not None: q[i] = quaternion_from_euler( np.radians(roll[i]), np.radians(pitch[i]), np.radians(yaw[i])) return q # Fixed (non-rotating) at roll 25 degrees, pitch 0 degrees, yaw 0 degrees # Gyro bias in x-direction of 0.1 rad/sec def generate_ground_truth_data1(t): N = len(t) roll = np.radians(25) pitch = np.radians(0) yaw = np.radians(0) q0 = quaternion_from_euler(roll, pitch, yaw) q = [q0] * N bx = [0.1] * N by = [0] * N bz = [0] * N return q, [bx, by, bz] # Rotating at constant angular rate [90 degrees/sec, 0, 0] # Gyro bias in x-direction of 0.1 rad/sec def generate_ground_truth_data2(t): N = len(t) q = [None] * N bx = [0.1] * N by = [0] * N bz = [0] * N q[0] = quaternion_identity() rate = 90 w = np.radians([rate, 0, 0]) 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, [bx, by, bz] # Rotating all axes at the same time def generate_ground_truth_data3(t): N = len(t) q = [None] * N bx = [0.1] * N by = [0.2] * N bz = [-0.1] * 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] w = np.radians([wx[i], wy[i], wz[i]]) 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, [bx, by, bz] GYRO_NOISE = 0.015 # rad/sec ACC_NOISE = 1.0 # m/s^2 # Generate time t0 = 0 tf = 10 dt = 0.01 t = np.arange(t0, tf, dt) # Generate ground truth orientation and bias #q, b = generate_ground_truth_data1(t) #q, b = generate_ground_truth_data2(t) q, b = generate_ground_truth_data3(t) # Generate sensor data a = generate_accelerometer_data_array(q, ACC_NOISE) w = generate_gyro_data_array(t, q, b, GYRO_NOISE) # EKF estimation estimated_q, estimated_b, b_err = 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, b, estimated_b, b_err)