卡尔曼滤波

把卡尔曼滤波写一下吧，思想很简单，不详细写了，就是根据方差实现的一种最优估计方法。

卡尔曼滤波五个基本的公式

1. 预测阶段

2. 校正阶段

代码详解

直接上代码，看代码的实现过程要更清晰有逻辑些
先放一个网址，解释的很详细。
另外一个卡尔曼滤波九轴融合算法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) A^T+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] 代入公式计算得

dt方值太小忽略掉

代码中的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) H^T / (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)

写这么多怪有成就感的，不过排版还是略累啊

卡尔曼滤波