开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记（三）
云长恨
接着说，看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); 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矩阵称为系统矩阵，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矩阵。
好，先写到这。
类似资料：