Quaternion-Based EKF for Attitude and Bias Estimation
URL: https://cookierobotics.com/073/
This page describes a method to estimate orientation given gyroscope andaccelerometer data. Extended Kalman filter (EKF) is used with quaternion andgyro bias 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 theprediction step. This is done because modeling the system dynamics accuratelyis difficult [1]. The measured angular velocity has bias applied to the trueangular velocity:
Accelerometer
Accelerometer data are assumed to be in m/s2 in the body frame:
Accelerometer data are treated as observations. The device acceleration isassumed to be small enough so that the accelerometer vector alwayspoints 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.
1g is 9.80665m/s2
State, x
State vector is:
The first four parameters are components of a unit quaternion and represents theorientation of the device. It is normalized every time it is modified.The last three parameters are gyro bias vector in rad/sec.
State Prediction, f(x)
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
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 theunit 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 vectorresults in the state transition matrix shown at the beginning of this section.
Process Noise
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.
Derivation of W
From the previous section, we know that
Solving the jacobian of the above with respect to the angular velocity results inW matrix shown at the beginning of this section.
Observation, z
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, 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 transposeof the above.
Observation model, H
Observation matrix is determined by taking the jacobian of the observationfunction:
Observation noise
Accelerometer noise appears as observation noise.Observation noise is given by,
where
σax, σay, σaz are accelerometer noise in m/s^2
Derivation of V
From the previous section, we know that
so
As a result, observation noise simplifies to R.
Python Implementation
filter.py
import numpy as npg = 9.80665def 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_unitdef 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
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 aboutZ-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 degreeswith 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.
Test 2: constant angular rate
IMU is rotated around the x-axis at constant angular rate of 90 degrees/sec withgyro bias in x-direction of 0.1 rad/sec.Roll is estimated correctly even with gyro bias.
Test 3: rotating all axes
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.
Source code for the above tests
It is assumed that filter.py is placed in the same folder as the below script.
import matplotlib.pyplot as pltimport numpy as npfrom filter import EKFfrom filter import quaternion_identityfrom filter import quaternion_multiplyfrom filter import quaternion_from_axis_anglefrom filter import normalizefrom filter import grandom_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, yawdef 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 wdef 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 adef 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, azdef 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, wzdef 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, yawdef 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/secdef 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/secdef 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 timedef 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/secACC_NOISE = 1.0 # m/s^2# Generate timet0 = 0tf = 10dt = 0.01t = 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 dataa = generate_accelerometer_data_array(q, ACC_NOISE)w = generate_gyro_data_array(t, q, b, GYRO_NOISE)# EKF estimationestimated_q, estimated_b, b_err = estimate(t, w, a)# Check resultsrpy = 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)
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
评论已关闭