根据先验估计值差分方程以及过程方程可以推导出先验误差的均方差,先验估计的协方差矩阵的正对角线就是其均方差,另外一条对角线上是其协方差,但是先验估计的协方差矩阵是如何推导出来的呢?
//Step 2 :先验估计 更新误差协方差矩阵 P
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt; |