当前位置: 首页 > 工具软件 > OpenPilot > 使用案例 >

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

云长恨
2023-12-01

接着说,看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
    q0 = X[6];
    q1 = X[7];
    q2 = X[8];
    q3 = X[9];

    // Pdot = V
    F[0][3] = F[1][4] = F[2][5] = 1.0f;

    // dVdot/dq
    F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
    F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
    F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
    F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az);
    F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
    F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az);
    F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
    F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
    F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
    F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
    F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az);
    F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az);

    // dVdot/dabias & dVdot/dna  - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
    // F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3);         F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
    // F[4][13]=G[4][3]=-2*(q1*q2+q0*q3);         F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
    // F[5][13]=G[5][3]=2*(-q1*q3+q0*q2);         F[5][14]=G[5][4]=-2*(q2*q3+q0*q1);         F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;

    // dqdot/dq
    F[6][6]  = 0;
    F[6][7]  = -wx / 2.0f;
    F[6][8]  = -wy / 2.0f;
    F[6][9]  = -wz / 2.0f;
    F[7][6]  = wx / 2.0f;
    F[7][7]  = 0;
    F[7][8]  = wz / 2.0f;
    F[7][9]  = -wy / 2.0f;
    F[8][6]  = wy / 2.0f;
    F[8][7]  = -wz / 2.0f;
    F[8][8]  = 0;
    F[8][9]  = wx / 2.0f;
    F[9][6]  = wz / 2.0f;
    F[9][7]  = wy / 2.0f;
    F[9][8]  = -wx / 2.0f;
    F[9][9]  = 0;

    // dqdot/dwbias
    F[6][10] = q1 / 2.0f;
    F[6][11] = q2 / 2.0f;
    F[6][12] = q3 / 2.0f;
    F[7][10] = -q0 / 2.0f;
    F[7][11] = q3 / 2.0f;
    F[7][12] = -q2 / 2.0f;
    F[8][10] = -q3 / 2.0f;
    F[8][11] = -q0 / 2.0f;
    F[8][12] = q1 / 2.0f;
    F[9][10] = q2 / 2.0f;
    F[9][11] = -q1 / 2.0f;
    F[9][12] = -q0 / 2.0f;

    // dVdot/dna  - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
    G[3][3]  = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
    G[3][4]  = 2.0f * (-q1 * q2 + q0 * q3);
    G[3][5]  = -2.0f * (q1 * q3 + q0 * q2);
    G[4][3]  = -2.0f * (q1 * q2 + q0 * q3);
    G[4][4]  = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
    G[4][5]  = 2.0f * (-q2 * q3 + q0 * q1);
    G[5][3]  = 2.0f * (-q1 * q3 + q0 * q2);
    G[5][4]  = -2.0f * (q2 * q3 + q0 * q1);
    G[5][5]  = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;

    // dqdot/dnw
    G[6][0]  = q1 / 2.0f;
    G[6][1]  = q2 / 2.0f;
    G[6][2]  = q3 / 2.0f;
    G[7][0]  = -q0 / 2.0f;
    G[7][1]  = q3 / 2.0f;
    G[7][2]  = -q2 / 2.0f;
    G[8][0]  = -q3 / 2.0f;
    G[8][1]  = -q0 / 2.0f;
    G[8][2]  = q1 / 2.0f;
    G[9][0]  = q2 / 2.0f;
    G[9][1]  = -q1 / 2.0f;
    G[9][2]  = -q0 / 2.0f;

    // dwbias = random walk noise
    G[10][6] = G[11][7] = G[12][8] = 1.0f;
好长一段代码,全贴出来是因为没法简单的说,代码就干一件事,给F和G矩阵赋值,这两个矩阵是雅可比矩阵,雅可比是非线性系统线性化的一种方法,基本我看过的卡尔曼程序都是这个方法,具体怎么算我也不知道,单位同事说好像挺复杂,反正这个矩阵就是这样了。

其中F矩阵称为系统矩阵,13×13维,就是我们的A阵,状态转移矩阵的线性化矩阵,对应我们的13维的状态变量,简单的说状态变量经过状态矩阵后就是下一步的状态变量。

G称为输入矩阵,13×9维,这里的输入是噪声,噪声和状态变量不一样,是个9×1维的矩阵,分别是三维陀螺仪噪声,三维加计噪声以及三维的偏差随机游走噪声(不知道什么偏差)。

我们知道,依据老外的现代控制理论,卡尔曼滤波的状态转移方程是:dotX=F*X+G*W。bolg不好写公式。

我们这里的F和G,就是上面公式里F和G的线性化。

好,LinearizeFG函数就这样吧,深入的我也不懂,反正这两个矩阵就这样了,拿过来用就可以了。

接着看RungeKutta函数,这个函数不用看也知道干啥的,龙格库塔解微分方程组,我进去看了一下,4阶定步长。

然后再跳出来,INSStatePrediction函数也走完了。

而else部分的剩余又是赋值,将解算完毕的状态变量赋值到外部可引用的数据结构中。然后else就结束了。

回头看一下,else部分只做了一件事,所谓的状态预测,就是解了一步微分方程组,也就是将陀螺仪和加计的数据积分一步,积分数据在长时间来说是不可信的,这个时间就MEMS陀螺仪来说不会很长,几秒或再长一点,就需要采用GPS,磁航向数据修正误差,让积分能够继续下去,修正部分还没看到,以后再说。

这篇博文拖了好长时间,主要是因为雅可比线性化的问题,也就是F矩阵里具体项的含义,这个矩阵表征物理模型,对滤波精度有很大影响,虽说大家都用一样的矩阵,但还是弄明白的好,我的打算是整个滤波流程走完了之后,再单独理解一下F矩阵。

好,先写到这。

 类似资料: