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 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