11mod:1_*___010_:10__Pr:*_*_kkkkkkkSystemelangleangledtgyromdtwangleqbiasqbiaswgyromeasurementangleanglevangleqbiasocessNoisedtQangledtQgyroMeasurementNoise1k|k-1:_Pr:()_*_**0001P=1011100011*_0011011010*_kRangleocessUpdateangleangleqbiasdtgyromdtangleRatedtPPPPdtPPdtdtQanglePPdtQgKalmanFilter红色角度预测：偏差预测：（就是它自己，不做运算）方差预测：200(_0110)11*()01(11)*1011*11*_:__0000110001110_11011010110yroPdtQanglePPPdtPPdtPPdtPdtQgyroMeasurementUpdateangleerrincAngleangleKPPPPKPPPP新息：增益(深蓝色)：1k|kk|k-1_00/(00_)100001_0P==I-10P1011_10001_000001=-1011_10101100-_0*0001RanglePPRanglePPPKPPKPPKPPPPKPPPKPP协方差更新（紫色）：（预测方差和方差可以共用内存，减少内存浪费）_0*0110_0*1011_1*01_0=+*____1KPPKPPKPangleangleKangleerrqbiasqbiasK状态估计： 

//float gyro_m：陀螺仪测得的量（角速度） //float incAngle:加速度计测得的角度值 #define dt 0.0015//卡尔曼滤波采样间隔 #define R_angle 0.69 //测量噪声的协方差 #define Q_angle 0.0001//角度噪声方差 #define Q_gyro 0.0003 //陀螺仪噪声方差 float kalmanUpdate(const float gyro_m,const float incAngle) { float K_0;//角度滤波增益 float K_1;//偏差滤波增益 float Y_0; float Y_1； float Rate;//去除偏差后的角速度 float Pdot[4];//计算状态预测协方差矩阵的中间矩阵 float angle_err;//角度偏量（新息） float E;//计算的中间量 static float angle = 0; //下时刻最优估计值角度 static float q_bias = 0; //陀螺仪的偏差（状态分量） static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//状态协方差矩阵 Rate = gyro_m - q_bias; // Kalman Time Update (状态更新) angle += Rate * dt; //作为预测 角度 //计算预测协方差矩阵（红色） Pdot[0] = Q_angle - P[0][1] - P[1][0]; Pdot[1] = - P[1][1]; Pdot[2] = - P[1][1]; Pdot[3] = Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; %% Innovation 信息序列 angle_err = incAngle - angle; //计算角度偏差 E = R_angle + P[0][0]; %% 滤波增益 K_0 = P[0][0] / E; K_1 = P[1][0] / E; //计算卡尔曼增益 // 更新新协方差矩阵 Y_0 = P[0][0]; Y_1 = P[0][1]; P[0][0] -= K_0 * Y_0; P[0][1] -= K_0 * Y_1; P[1][0] -= K_1 * Y_0; P[1][1] -= K_1 * Y_1; angle += K_0 * angle_err; //给出最优估计值 q_bias += K_1 * angle_err; //给出最优估计值偏差 return angle;}