卡尔曼滤波算法相关问题

葫芦导弹

合工大握手······挖坟抱歉了哈哈，师兄你的程序第一句是不是应该移到这段程序最后啊，RATE没赋初值啊，还有卡尔曼响应太慢是怎么回事，修改之后还是慢

       angle += Rate * dt; //角速度积分得出角度
       P[0][0] += Pdot[0] * dt; //计算协方差矩阵             | 1.00002      -0.02 |
       P[0][1] += Pdot[1] * dt; //                           |- 0.02      1.00006 |
       P[1][0] += Pdot[2] * dt;
       P[1][1] += Pdot[3] * dt;
       Rate = gyro_m - q_bias;// 去除偏差后的角速度