接着说,看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