卡尔曼滤波数据融合算法.doc
2页
/*********************************************************// 卡尔曼滤波//*********************************************************//在程序中利用 Angle+=(Gyro - Q_bias) * dt计算出陀螺仪积分出的角度， 其中Q_bias是陀螺仪偏差//此时利用陀螺仪积分求出的 Angle相当于系统的估计值，得到系统的观测方程；而加速度计检测的角度Accel相当于系统中的测量值，得到系统状态方程//程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度根据 Pdot = A*P +P*A' + Q_angle 计算出先验估计协方差的微分，用于将当前估计值进行线性化处理其中 A为雅克比矩阵//随后计算系统预测角度的协方差矩阵 P计算估计值 Accel与预测值 Angle间的误差Angle_err//计算卡尔曼增益 K_0,K_1, K_0用于最优估计值，K_1用于计算最优估计值的偏差并更新协 方差矩阵 P//通过卡尔曼增益计算出最优估计值 Angle及预测值偏差 Q_bias，此时得到最优角度值 Angle及角度值。
//Kalman 滤波， 20MHz 的处理时间约 0.77ms ；void Kalman_Filter(float Accel,float Gyro){Angle+=(Gyro - Q_bias) * dt; // 先验估计Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk- 先验估计误差协方差的微分Pdot[1]=- PP[1][1];// Pk-先验估计误差协方差微分的积分// =先验估计误差协方差//zk- 先验估计Pdot[2]=- PP[1][1]; Pdot[3]=Q_gyro;PP[0][0] += Pdot[0] * dt;PP[0][1] += Pdot[1] * dt;PP[1][0] += Pdot[2] * dt;PP[1][1] += Pdot[3] * dt;Angle_err = Accel - Angle;PCt_0 = C_0 * PP[0][0];PCt_1 = C_0 * PP[1][0];E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * PP[0][1];PP[0][0] -= K_0 * t_0;PP[0][1] -= K_0 * t_1;PP[1][0] -= K_1 * t_0;PP[1][1] -= K_1 * t_1;// 后验估计误差协方差Angle += K_0 * Angle_err; // 后验估计Q_bias += K_1 * Angle_err; // 后验估计Gyro_y = Gyro - Q_bias; //输出值（后验估计 ）的微分=角度。
