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

最新推荐文章于 2024-06-20 12:02:08 发布

romepop1

最新推荐文章于 2024-06-20 12:02:08 发布

阅读量4.4k

点赞数 2

文章标签： filter 卡尔曼飞控开源

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

版权

接着说，看else部分。

 float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
 INSStatePrediction(gyros, this->work.accel, dT);

先看前两句，第一句不用说，给gyros[3]数组赋值，重点看第二句，从字面来看，是进行INS的状态预测，事实也是如此，进入到函数内部去看

    LinearizeFG(ekf.X, U, ekf.F, ekf.G);
    RungeKutta(ekf.X, U, dT);

函数中重点是以上两句话，其余都是赋值与被赋值，不去看，首先看第一句，字面上看是线性化某物，再进入函数看一下

 float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;

    // ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15];  // subtract the biases on accels
    ax = U[3];
    ay = U[4];
    az = U[5]; // NO BIAS STATES ON ACCELS
    wx = U[0] - X[10];
    wy = U[1] - X[11];
    wz = U[2] - X[12]; // subtract the biases on gyros