AHRS Extended Kalman filter

The extended Kalman filter(EKF) is the nonlinear version of the Kalman filter. EKF has 'Predict' and 'Update' stages and recursivley estimates target values.

enum { EKF_X = 0, EKF_Y, EKF_Z, EKF_W };
static const float EKF_Q_ROT = 0.1f;
static const float EKF_R_ACC = 200.0f;
static const float EKF_R_MAG = 2000.0f;
static float quaternion[4] = {0.0, 0.0, 0.0, 1.0};
static float P[4][4] = {{1.0, 0.0, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 1.0}};
static float F[4][4];
static float y[6];
static float H[6][4];
static float S[6][6];
static float K[4][6];
void ahrs_get_unit_vector(float *vector, uint16_t vector_size) {
double sq_sum = 0;
for(uint16_t i = 0; i < vector_size; ++i) {
sq_sum += (vector[i] * vector[i]);
}
float norm = sqrt(sq_sum);
for(uint16_t i = 0; i < vector_size; ++i) { vector[i] /= norm; }
}

Predict

xk=qk0=[qxqyqzqw]Tx_k = q^0_k = \begin{bmatrix} q_x & q_y & q_z & q_w \end{bmatrix}^T
qkkβˆ’1β‰ˆ[Ο‰xΞ”t2Ο‰yΞ”t2Ο‰zΞ”t21]Tq^{k-1}_k \approx \begin{bmatrix} \dfrac {\omega_x \Delta t} {2} & \dfrac {\omega_y \Delta t} {2} & \dfrac {\omega_z \Delta t} {2} & 1 \end{bmatrix}^T
x^k∣kβˆ’1=f(x^kβˆ’1∣kβˆ’1,uk)=x^kβˆ’1∣kβˆ’1qkkβˆ’1\hat{x}_{k|k-1} = f (\hat{x}_{k-1|k-1},u_k) = \hat{x}_{k-1|k-1}q^{k-1}_k
x^k∣kβˆ’1=R(qkkβˆ’1)x^kβˆ’1∣kβˆ’1=[1Ο‰zΞ”t2βˆ’Ο‰yΞ”t2Ο‰xΞ”t2βˆ’Ο‰zΞ”t21Ο‰xΞ”t2Ο‰yΞ”t2Ο‰yΞ”t2βˆ’Ο‰xΞ”t21Ο‰zΞ”t2βˆ’Ο‰xΞ”t2βˆ’Ο‰yΞ”t2βˆ’Ο‰zΞ”t21]x^kβˆ’1∣kβˆ’1\hat{x}_{k|k-1} = R(q^{k-1}_k) \hat{x}_{k-1|k-1} = \begin{bmatrix} 1 & \dfrac {\omega_z \Delta t} {2} & - \dfrac {\omega_y \Delta t} {2} & \dfrac {\omega_x \Delta t} {2} \\ - \dfrac {\omega_z \Delta t} {2} & 1 & \dfrac {\omega_x \Delta t} {2} & \dfrac {\omega_y \Delta t} {2} \\ \dfrac {\omega_y \Delta t} {2} & - \dfrac {\omega_x \Delta t} {2} & 1 & \dfrac {\omega_z \Delta t} {2} \\ - \dfrac {\omega_x \Delta t} {2} & - \dfrac {\omega_y \Delta t} {2} & - \dfrac {\omega_z \Delta t} {2} & 1 \end{bmatrix} \hat{x}_{k-1|k-1}
Fk=βˆ‚fβˆ‚x∣x^kβˆ’1∣kβˆ’1,uk=[1Ο‰zΞ”t2βˆ’Ο‰yΞ”t2Ο‰xΞ”t2βˆ’Ο‰zΞ”t21Ο‰xΞ”t2Ο‰yΞ”t2Ο‰yΞ”t2βˆ’Ο‰xΞ”t21Ο‰zΞ”t2βˆ’Ο‰xΞ”t2βˆ’Ο‰yΞ”t2βˆ’Ο‰zΞ”t21]F_k =\left. \dfrac{\partial f}{\partial x} \right|_{\hat{x}_{k-1|k-1},u_k} = \begin{bmatrix} 1 & \dfrac {\omega_z \Delta t} {2} & - \dfrac {\omega_y \Delta t} {2} & \dfrac {\omega_x \Delta t} {2} \\ - \dfrac {\omega_z \Delta t} {2} & 1 & \dfrac {\omega_x \Delta t} {2} & \dfrac {\omega_y \Delta t} {2} \\ \dfrac {\omega_y \Delta t} {2} & - \dfrac {\omega_x \Delta t} {2} & 1 & \dfrac {\omega_z \Delta t} {2} \\ - \dfrac {\omega_x \Delta t} {2} & - \dfrac {\omega_y \Delta t} {2} & - \dfrac {\omega_z \Delta t} {2} & 1 \end{bmatrix}
Pk∣kβˆ’1=FkPkβˆ’1∣kβˆ’1FkT+QkP_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_k
void ahrs_predict(float *half_delta_angle) {
/*
* x_(k|k-1) = f(x_(k-1|k-1))
*/
float delta_quaternion[4];
delta_quaternion[EKF_X] = half_delta_angle[EKF_Z] * quaternion[EKF_Y]
- half_delta_angle[EKF_Y] * quaternion[EKF_Z]
+ half_delta_angle[EKF_X] * quaternion[EKF_W];
delta_quaternion[EKF_Y] = -half_delta_angle[EKF_Z] * quaternion[EKF_X]
+ half_delta_angle[EKF_X] * quaternion[EKF_Z]
+ half_delta_angle[EKF_Y] * quaternion[EKF_W];
delta_quaternion[EKF_Z] = half_delta_angle[EKF_Y] * quaternion[EKF_X]
- half_delta_angle[EKF_X] * quaternion[EKF_Y]
+ half_delta_angle[EKF_Z] * quaternion[EKF_W];
delta_quaternion[EKF_W] = -half_delta_angle[EKF_X] * quaternion[EKF_X]
- half_delta_angle[EKF_Y] * quaternion[EKF_Y]
- half_delta_angle[EKF_Z] * quaternion[EKF_Z];
quaternion[EKF_X] += delta_quaternion[EKF_X];
quaternion[EKF_Y] += delta_quaternion[EKF_Y];
quaternion[EKF_Z] += delta_quaternion[EKF_Z];
quaternion[EKF_W] += delta_quaternion[EKF_W];
/*
* F = df/dx
*/
F[0][0] = 1.0;
F[0][1] = half_delta_angle[EKF_Z];
F[0][2] = -half_delta_angle[EKF_Y];
F[0][3] = half_delta_angle[EKF_X];
F[1][0] = -half_delta_angle[EKF_Z];
F[1][1] = 1.0;
F[1][2] = half_delta_angle[EKF_X];
F[1][3] = half_delta_angle[EKF_Y];
F[2][0] = half_delta_angle[EKF_Y];
F[2][1] = -half_delta_angle[EKF_X];
F[2][2] = 1.0;
F[2][3] = half_delta_angle[EKF_Z];
F[3][0] = -half_delta_angle[EKF_X];
F[3][1] = -half_delta_angle[EKF_Y];
F[3][2] = -half_delta_angle[EKF_Z];
F[3][3] = 1.0;
/*
* P_(k|k-1) = F*P_(k-1|k-1)*F^T + Q
* K is used instead of F*P_(k-1|k-1)
*/
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 4; ++j) {
K[i][j] = F[i][0] * P[0][j]
+ F[i][1] * P[1][j]
+ F[i][2] * P[2][j]
+ F[i][3] * P[3][j];
}
}
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 4; ++j) {
P[i][j] = K[i][0] * F[j][0]
+ K[i][1] * F[j][1]
+ K[i][2] * F[j][2]
+ K[i][3] * F[j][3];
}
P[i][i] += EKF_Q_ROT;
}
ahrs_get_unit_vector(quaternion, 4);
}

Update

0a=[00g0]T{}^0a = \begin{bmatrix} 0 & 0 & g & 0 \end{bmatrix}^T
ka^βˆ₯ka^βˆ₯=ha(x^k∣kβˆ’1)=x^k∣kβˆ’1β€Ύ(0aβˆ₯0aβˆ₯)x^k∣kβˆ’1=[2(qxqzβˆ’qyqw)2(qxqw+qyqz)βˆ’qx2βˆ’qy2+qz2+qw2]\begin{aligned} \dfrac{{}^k\hat{a}} {\lVert {}^k\hat{a} \rVert} &= h_a(\hat{x}_{k|k-1}) \\ &= \overline{\hat{x}_{k|k-1}} \left( \dfrac{{}^0a} {\lVert {}^0a \rVert} \right) \hat{x}_{k|k-1} = \begin{bmatrix} 2(q_x q_z - q_y q_w) \\ 2(q_x q_w + q_y q_z) \\ -q^2_x - q^2_y + q^2_z + q^2_w \end{bmatrix} \end{aligned}
0m=[0mx00mz0]T{}^0m = \begin{bmatrix} {}^0m_x & 0 & {}^0m_z & 0 \end{bmatrix}^T
0aΓ—0m=[00mxg00]T{}^0a \times {}^0m = \begin{bmatrix} 0 & {}^0m_x g & 0 & 0 \end{bmatrix}^T
ka^Γ—km^βˆ₯ka^Γ—km^βˆ₯=haΓ—m(x^k∣kβˆ’1)=x^k∣kβˆ’1β€Ύ(0aΓ—0mβˆ₯0aΓ—0mβˆ₯)x^k∣kβˆ’1=[2(qxqy+qzqw)βˆ’qx2+qy2βˆ’qz2+qw22(qyqzβˆ’qxqw)]\begin{aligned} \dfrac{{}^k\hat{a} \times {}^k\hat{m}} {\lVert {}^k\hat{a} \times {}^k\hat{m} \rVert} &= h_{a \times m}( \hat{x}_{k|k-1} ) \\ &= \overline{\hat{x}_{k|k-1}} \left( \dfrac{{}^0a \times {}^0m} {\lVert {}^0a \times {}^0m \rVert} \right) \hat{x}_{k|k-1} = \begin{bmatrix} 2(q_x q_y + q_z q_w) \\ -q^2_x + q^2_y - q^2_z + q^2_w \\ 2(q_y q_z - q_x q_w) \end{bmatrix} \end{aligned}
zk=[kaβˆ₯kaβˆ₯kaΓ—kmβˆ₯kaΓ—kmβˆ₯]h(x^k∣kβˆ’1)=[ha(x^k∣kβˆ’1)haΓ—m(x^k∣kβˆ’1)]z_k = \begin{bmatrix} \dfrac{{}^ka} {\lVert {}^ka \rVert} \\ \dfrac{{}^ka \times {}^km} {\lVert {}^ka \times {}^km \rVert} \end{bmatrix} \quad h( \hat{x}_{k|k-1} ) = \begin{bmatrix} h_a(\hat{x}_{k|k-1}) \\ h_{a \times m}( \hat{x}_{k|k-1} ) \end{bmatrix}
y~k=zkβˆ’h(x^k∣kβˆ’1)\tilde{y}_k = z_k - h( \hat{x}_{k|k-1} )
Hk=2[qzβˆ’qwqxβˆ’qyqwqzqyqxβˆ’qxβˆ’qyqzqwqyqxqwqzβˆ’qxqyβˆ’qzqwβˆ’qwqzqyβˆ’qx]H_k = 2 \begin{bmatrix} q_z & -q_w & q_x & -q_y \\ q_w & q_z & q_y & q_x \\ -q_x & -q_y & q_z & q_w \\ q_y & q_x & q_w & q_z \\ -q_x & q_y & -q_z & q_w \\ -q_w & q_z & q_y & -q_x \end{bmatrix}
Sk=HkPk∣kβˆ’1HkT+RkS_k = H_k P_{k|k-1} H^T_k + R_k
Kk=Pk∣kβˆ’1HkTSkβˆ’1K_k = P_{k|k-1} H^T_k S^{-1}_k
Pk∣k=(Iβˆ’KkHk)Pk∣kβˆ’1P_{k|k} = (I - K_k H_k) P_{k|k-1}
x^k∣k=x^k∣kβˆ’1+Kky~k\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k
void ahrs_update(float *unit_acc, float *unit_acc_cross_mag) {
/*
* y = z - h(x_(k|k-1))
*/
y[0] = unit_acc[EKF_X]
- 2 * (quaternion[EKF_X] * quaternion[EKF_Z]
- quaternion[EKF_Y] * quaternion[EKF_W]);
y[1] = unit_acc[EKF_Y]
- 2 * (quaternion[EKF_X] * quaternion[EKF_W]
+ quaternion[EKF_Y] * quaternion[EKF_Z]);
y[2] = unit_acc[EKF_Z]
- (1 - 2 * (quaternion[EKF_X] * quaternion[EKF_X]
+ quaternion[EKF_Y] * quaternion[EKF_Y]));
y[3] = unit_acc_cross_mag[EKF_X]
- 2 * (quaternion[EKF_X] * quaternion[EKF_Y]
+ quaternion[EKF_Z] * quaternion[EKF_W]);
y[4] = unit_acc_cross_mag[EKF_Y]
- (1 - 2 * (quaternion[EKF_X] * quaternion[EKF_X]
+ quaternion[EKF_Z] * quaternion[EKF_Z]));
y[5] = unit_acc_cross_mag[EKF_Z]
- 2 * (quaternion[EKF_Y] * quaternion[EKF_Z]
- quaternion[EKF_X] * quaternion[EKF_W]);
/*
* H = dh/dx
*/
H[0][0] = 2 * quaternion[EKF_Z];
H[0][1] = -2 * quaternion[EKF_W];
H[0][2] = 2 * quaternion[EKF_X];
H[0][3] = -2 * quaternion[EKF_Y];
H[1][0] = -H[0][1];
H[1][1] = H[0][0];
H[1][2] = -H[0][3];
H[1][3] = H[0][2];
H[2][0] = -H[0][2];
H[2][1] = H[0][3];
H[2][2] = H[0][0];
H[2][3] = H[1][0];
H[3][0] = H[1][2];
H[3][1] = H[0][2];
H[3][2] = H[1][0];
H[3][3] = H[0][0];
H[4][0] = H[2][0];
H[4][1] = H[1][2];
H[4][2] = -H[0][0];
H[4][3] = H[1][0];
H[5][0] = H[0][1];
H[5][1] = H[0][0];
H[5][2] = H[1][2];
H[5][3] = H[2][0];
/*
* S = H*P_(k|k-1)*(H^T) + R
* K is used instead of P_(k|k-1)*(H^T)
*/
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 6; ++j) {
K[i][j] = P[i][0] * H[j][0]
+ P[i][1] * H[j][1]
+ P[i][2] * H[j][2]
+ P[i][3] * H[j][3];
}
}
for(uint16_t i = 0; i < 6; ++i) {
for(uint16_t j = 0; j < 6; ++j) {
S[i][j] = H[i][0] * K[0][j]
+ H[i][1] * K[1][j]
+ H[i][2] * K[2][j]
+ H[i][3] * K[3][j];
}
if(i < 3) S[i][i] += EKF_R_ACC;
else S[i][i] += EKF_R_MAG;
}
/*
* K = P_(k|k-1)*(H^T)*(S^-1)
* K*S = P_(k|k-1)*(H^T)
*
* Cholesky decomposition S=L*(L^*T) (L^*T == conjugate transpose L)
* S is used instead of L
*/
for(uint16_t i = 0; i < 6; ++i) {
for(uint16_t j = 0; j < i; ++j) {
for(uint16_t k = 0; k < j; ++k) { S[i][j] -= S[i][k] * S[j][k]; }
S[i][j] /= S[j][j];
S[i][i] -= S[i][j] * S[i][j];
}
S[i][i] = sqrt(S[i][i]);
}
/*
* L*(L^*T)*(K^T) = (P_(k|k-1)*(H^T))^T
* L*A = (P_(k|k-1)*(H^T))^T
* K is used instead of A
*/
for(uint16_t k = 0; k < 4; ++k) {
for(uint16_t i = 0; i < 6; ++i) {
for(uint16_t j = 0; j < i; ++j) { K[k][i] -= S[i][j] * K[k][j]; }
K[k][i] /= S[i][i];
}
}
/*
* (L^*T)*(K^T) = A
*/
for(uint16_t k = 0; k < 4; ++k) {
for(uint16_t i = 0; i < 6; ++i) {
for(uint16_t j = 0; j < i; ++j) {
K[k][5 - i] -= S[5 - j][5 - i] * K[k][5 - j];
}
K[k][5 - i] /= S[5 - i][5 - i];
}
}
/*
* x = x + K*y
*/
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 6; ++j) { quaternion[i] += (K[i][j] * y[j]); }
}
ahrs_get_unit_vector(quaternion, 4);
/*
* P_(k|k) = (I-K*H)*P_(k|k-1)
* F is used instead of K*H
*/
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 4; ++j) {
F[i][j] = K[i][0] * H[0][j]
+ K[i][1] * H[1][j]
+ K[i][2] * H[2][j]
+ K[i][3] * H[3][j];
}
}
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 4; ++j) {
K[i][j] = F[i][0] * P[0][j]
+ F[i][1] * P[1][j]
+ F[i][2] * P[2][j]
+ F[i][3] * P[3][j];
}
}
for(uint16_t i = 0; i < 4; ++i) {
for(uint16_t j = 0; j < 4; ++j) { P[i][j] -= K[i][j]; }
}
}

Reference

  • Matthew Watson, Luke Seed, and Chee Hing Tan, 2013, β€œThe Design and Implementation of a robust AHRS for Integration into a Quadrotor Platform.”, p.14 ~ p.34
  • Heikki Hyyiti, and Arto Visala, 2015, β€œA DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs”, p.4 ~ p.5
Last updated on