《卡尔曼滤波数据融合算法》由会员分享，可在线阅读，更多相关《卡尔曼滤波数据融合算法（2页珍藏版）》请在金锄头文库上搜索。
1、/*/ 卡尔曼滤波/*/在程序中利用 Angle+=(Gyro - Q_bias) * dt计算出陀螺仪积分出的角度，其中Q_bias是陀螺仪偏差。/此时利用陀螺仪积分求出的Angle相当于系统的估计值，得到系统的观测方程；而加速度计检测的角度Accel相当于系统中的测量值，得到系统状态方程。/程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度。根据Pdot = A*P +P*A + Q_angle 计算出先验估计协方差的微分，用于将当前估计值进行线性化处理。其中A为雅克比矩阵。/随后计算系统预测角度的协方差矩阵P。计算估计值 Accel与预测值 Angle间的误差Ang
2、le_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; / 先验估计Pdot0=Q_angle - PP01 - PP10; / Pk- 先验估计误差协方差的微分Pdot1=- PP11;/ Pk-先验估计误差协方
3、差微分的积分/ =先验估计误差协方差/zk- 先验估计Pdot2=- PP11; Pdot3=Q_gyro;PP00 += Pdot0 * dt;PP01 += Pdot1 * dt;PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;Angle_err = Accel - Angle;PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;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 * PP01;PP00 -= K_0 * t_0;PP01 -= K_0 * t_1;PP10 -= K_1 * t_0;PP11 -= K_1 * t_1;/ 后验估计误差协方差Angle+= K_0 * Angle_err;/ 后验估计Q_bias+= K_1 * Angle_err;/ 后验估计Gyro_y = Gyro - Q_bias;/输出值（后验估计 ）的微分=角度
