把卡尔曼滤波写一下吧，思想很简单，不详细写了，就是根据方差实现的一种最优估计方法。
卡尔曼滤波五个基本的公式
1. 预测阶段
1. 1 X(k|k-1)=AX(k-1|k-1)+BU(k)..........先验估计
A,B为矩阵系数，X(k|k-1)是根据上一状态预测的结果，X(k-1|k-1)是上一次的最优结果，U(k)为现在状态的控制量。
1. 2 P(k|k-1)=A P(k-1| k-1) AT+Q..........误差协方差
P(k|k-1)是X(k|k-1)的协方差，P(k-1| k-1) 是X(k-1|k-1)的协方差，Q是估计过程的误差协方差。
2. 校正阶段
2. 1 Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)..........计算卡尔曼增益
H也为系数矩阵，R为测量值的噪声协方差。
2. 2 X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))..........修正估计
Z(K)是测量值。
2. 3 P(k|k)=（ I-Kg(k) H） P(k|k-1)..........更新误差协方差
*I 是为1的矩阵，对于单模型单测量I *为1.
代码详解
直接上代码，看代码的实现过程要更清晰有逻辑些
先放一个网址，解释的很详细。
另外一个卡尔曼滤波九轴融合算法stm32尝试
以下是一个平衡车的卡尔曼滤波代码：
float Q_angle=0.001; //陀螺仪噪声的协方差
float Q_gyro=0.003; //陀螺仪漂移噪声的协方差
float R_angle=0.5; // 加速度计的协方差
float dt=0.005;
char C_0 = 1;
float Q_bias=0, Angle_err=0; //Q_bias为陀螺仪漂移
float PCt_0=0, PCt_1=0, E=0;
float K_0=0, K_1=0, t_0=0, t_1=0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter(float Gyro,float Accel)
{ //Gyro陀螺仪的测量值，Accel加速度计的角度计算值
Angle+=(Gyro - Q_bias) * dt;
//角度测量模型方程
//就漂移来说认为每次都是相同的Q_bias=Q_bias；
//由此得到矩阵
以上代码对应第一个公式X(k|k-1)=AX(k-1|k-1)+BU(k)
Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
Pdot[1] = -PP[1][1];
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;
以上代码对应第二个公式P(k|k-1)=A P(k-1| k-1) AT+Q
公式中Q为向量A的协方差矩阵
因为漂移噪声和角度噪声是相互独立的，所以cov(x，y)=0,又因为cov(x,x)=D(x),方差值Q_angle,Q_bias程序开头定义已给出
卡尔曼滤波的目标就是使P(k-1| k-1)值最小，设为[a,b; c,d] 代入公式计算得
代码中的
Pdot为矩阵计算得中间变量，PP对应a,b,c,d
PCt_0 = C_0 * PP[0][0];//矩阵乘法的中间变量
PCt_1 = C_0 * PP[1][0];//C_0=1
//分母
E = R_angle + C_0 * PCt_0;
//增益值
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
以上代码对应第三个公式Kg(k)= P(k|k-1) HT / (H P(k|k-1)
这里卡尔曼增益是一个二维向量|k0, k1|T，H=|1, 0|,R即为加速度计测量出角度值的噪声
Angle_err = Accel - Angle;
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
Gyro_x = Gyro - Q_bias; //计算得角速度值
以上对应第四个公式X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))，代入相应值即可
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;
}
第五个公式P(k|k)=（ I-Kg(k) H） P(k|k-1)
写这么多怪有成就感的，不过排版还是略累啊
