Skip to main content

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 qkk1[ω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^kk1=f(x^k1k1,uk)=x^k1k1qkk1\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^kk1=R(qkk1)x^k1k1=[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^k1k1\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=fxx^k1k1,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} Pkk1=FkPk1k1FkT+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^kk1)=x^kk1(0a0a)x^kk1=[2(qxqzqyqw)2(qxqw+qyqz)qx2qy2+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^kk1)=x^kk1(0a×0m0a×0m)x^kk1=[2(qxqy+qzqw)qx2+qy2qz2+qw22(qyqzqxqw)]\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=[kakaka×kmka×km]h(x^kk1)=[ha(x^kk1)ha×m(x^kk1)]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=zkh(x^kk1)\tilde{y}_k = z_k - h( \hat{x}_{k|k-1} ) Hk=2[qzqwqxqyqwqzqyqxqxqyqzqwqyqxqwqzqxqyqzqwqwqzqyqx]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=HkPkk1HkT+RkS_k = H_k P_{k|k-1} H^T_k + R_k Kk=Pkk1HkTSk1K_k = P_{k|k-1} H^T_k S^{-1}_k Pkk=(IKkHk)Pkk1P_{k|k} = (I - K_k H_k) P_{k|k-1} x^kk=x^kk1+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