开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记（五）

最新推荐文章于 2024-08-28 19:18:44 发布

romepop1

最新推荐文章于 2024-08-28 19:18:44 发布

阅读量4.2k

点赞数 2

本文链接：https://blog.csdn.net/romepop1/article/details/41251269

版权

本文是关于开源飞控OpenPilot中扩展卡尔曼滤波（EKF）的学习笔记，主要探讨了滤波修正过程，包括线性化H矩阵、预测观测量的计算以及状态变量的修正和更新。作者通过分析代码详细解释了EKF的实现步骤，并预告下篇将深入讲解EKF的理论基础。

摘要由CSDN通过智能技术生成

出差归来，接着说，上回书说到函数INSCorrection，这个函数进行具体的滤波修正。

INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors);

进去看看。

    float Z[10], Y[10];
    float Bmag, qmag;

    // GPS Position in meters and in local NED frame
    Z[0] = Pos[0];
    Z[1] = Pos[1];
    Z[2] = Pos[2];

    // GPS Velocity in meters and in local NED frame
    Z[3] = Vel[0];
    Z[4] = Vel[1];
    Z[5] = Vel[2];

    // magnetometer data in any units (use unit vector) and in body frame
    Bmag =
        sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
              mag_data[2] * mag_data[2]);
    Z[6] = mag_data[0] / Bmag;
    Z[7] = mag_data[1] / Bmag;
    Z[8] = mag_data[2] / Bmag;

    // barometric altimeter in meters and in local NED frame
    Z[9] = BaroAlt;

先是一段赋值，我们知道Z阵是新息，这里先把所有能用到的传感器数据赋值到新息矩阵。

    LinearizeH(ekf.X, ekf.Be, ekf.H);

然后是对H阵进行线性化，难道H阵不是1吗，进去看看

    float q0, q1, q2, q3;

    q0 = X[6];
    q1 = X[7];
    q2 = X[8];
    q3 = X[9];

    // dP/dP=I;
    H[0][0] = H[1][1] = H[2][2] = 1.0f;
    // dV/dV=I;
    H[3][3] = H[4][4] = H[5][5] = 1.0f;

    // dBb/dq
    H[6][6] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
    H[6][7] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
    H[6][8] = 2.0f * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
    H[6][9] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
    H[7][6] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
    H[7][7] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
    H[7][8] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
    H[7][9] = 2.0f * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
    H[8][6] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
    H[8][7] = 2.0f * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
    H[8][8] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
    H[8][9] &