卡尔曼滤波里面协方差矩阵怎么搞
就是下面这段，P_00，P_01，P_10，P_11这部分看不懂// 卡尔曼滤波
Klm_angle += (omega - bias) * dt; // 先验估计
P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
P_01 += -P_11 * dt;
P_10 += -P_11 * dt;
P_11 += +Q_omega * dt; // 先验估计误差协方差
float K_0 = P_00 / (P_00 + R_angle);
float K_1 = P_10 / (P_00 + R_angle);
bias += K_1 * (angleA - Klm_angle);
Klm_angle += K_0 * (angleA - Klm_angle); // 后验估计
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01; // 后验估计误差协方差
页:
[1]