对于Kalman滤波器的简单理解

        简单说下，贝叶斯两大分支之一的卡尔曼家族的小弟，线性高斯滤波器（KF）。

        何为线性？自然指的是系统是线性的（系统方程满足齐次性、叠加性）。

        何为高斯？自然指的是噪声服从高斯分布。

        核心：通过计算卡尔曼增益，去加权融合观测结果（量测）与预测结果（估计）；一般，量测是观测模型带来的结果，预测是运动模型带来的结果。      

        主要分为预测、更新两大步骤。

        过程：预测部分，根据t-1时刻的后验估计和后验协方差更新当前t时刻的先验估计和先验协方差矩阵；更新部分，加入观测数据，更新t时刻卡尔曼增益，加权状态的先验值和残差（红色框）得到后验状态值，更新后验协方差矩阵；     

        设系统状态向量为x，系统状态的不确定性是通过协方差矩阵去反应，而协方差矩阵Pt的转移是卡尔曼滤波器很关键的一步骤，它是通过系统的状态转移矩阵Ft，如下所示：

       

        状态转移矩阵F、以及状态控制矩阵B的获取，一般是通过构建系统对于状态量的物理学运动方程获得。Q、R分别是预测部分（运动方程噪声Wk~N(0, Q)）和观测部分(观测方程噪声Vk~(0, R))的噪声方差。

附加：高斯分布N(μ，σ2)

工程上，一般取正负3*σ（3倍标准差），偏离均值3σ的数据就不要了。

         正态分布的概率密度函数，对称轴为均值μ（期望），方差σ2（代表整个数据集合的误差水平）。最高点值为均值对应的概率密度。正态分布的随机变量取与均值邻近的值的概率大（粉色区区域），取离均值远的值的概率小。各个数据与平均数的差的平方和较大，分布则比较散开；曲线与横轴积分值为1；

        0均值的高斯分布N(0，σ2)：对于卡尔曼滤波器而言，噪声服从0均值不相关的高斯分布Wk~N(0, Q)、Vk~(0, R)，则卡尔曼滤波器得到的解便是最优解（无偏最优估计）；

        标准高斯分布N(0，1)；

        从概率角度使用最大后验概率估计的形式（后验=似然*先验），以实际数值感受一下推导过程。假设现在两个高斯分布，一个为观测数据的分布，一个为预测数据的分布，那么如何得到kalman认为的最优估计值呢？

        首先，我们认为我们要求解的最优估计值也是服从高斯分布的，并且该高斯分布的均值，就是我们要寻找的值。那么kalman如何做呢？即使将观测数据的分布与预测数据的分布相乘。

        

 假设两组数据计算如下：

 其中，红色框中的即为Kalman增益。

理解下图的更新加权过程：

实际使用中可能的问题：在长时间丢失量测数据或者系统状态处于可观测性弱，由于系统噪声的不断输入，系统的协方差矩阵会越变越大。为了防止卡尔曼滤波的协方差矩阵出现病态，根据实际情况要将卡尔曼的协方差矩阵进行阈值限定，防止协方差矩阵无限扩大；

使用KF进行陀螺仪角度融合代码：

 class kfForImuFilter {

public:
    double angle;           //系统状态向量（angle角度，Q_bias角速度偏移）
    double Q_bias;
    double X[2][1] = {angle, Q_bias};

private:
    bool isfirst;           //系统第一次进行，将量测数据作为最优估计
    double Q_angle = 0.001; //系统状态的方差，状态不相关，所以两状态之间的协方差为0，见P矩阵
    double Q_gyro = 0.003;
    double R_angle = 0.5;   //观测误差（观测量：角度）

    double dt = 0.002;      //采样时间（用于积分）

    double A[2][2] = {  //系统的状态转移矩阵
        1, -dt,
        0, 1
    };
    double P[2][2] = {  //系统状态协方差矩阵
        Q_angle, 0,
        0, Q_gyro
    };
    double H[1][2] = {  //测量矩阵
        1, 0
    };
    double R[1][1] = {  //量测的噪声矩阵
        R_angle
    };
    double K[2][1];     //卡尔曼增益

public:
    kfForImuFilter(){
        isfirst = true;
    }
    ~kfForImuFilter(){}

    /*
     * newAngle ：量测-角度（陀螺仪输出直接计算的角度）
     *  newGyro ：模型估计-角速度
     */
    void kfForImuFilterProcess(double newAngle, double newGyro){

        if(isfirst){    // 初始状态
            X[0][0] = newAngle;
            X[1][0] = 0;
            isfirst = false;
        }

    /**********预测************/
    // 模型预测当前状态
        X[0][0] = X[0][0] + (newGyro-X[1][0])*dt;
        X[1][0] = X[1][0];	

    // 协方差矩阵传播
        P[0][0] += Q_angle - (P[0][1] - P[1][0])*dt; // + P[1][1]*pow(dt,2) //由于此项太小可以忽略
        P[0][1] += -P[1][1]*dt;
        P[1][0] += -P[1][1]*dt;
        P[1][1] = P[1][0] + Q_gyro;
		
    /**********更新************/
    // 开始使用量测数据
    // 计算卡尔曼增益
        K[0][0] = P[0][0] / (P[0][0]+R[0][0]);
        K[1][0] = P[1][0] / (P[0][0]+R[0][0]);

    // 计算当前状态的最优估计
        X[0][0] += K[0][0]*(newAngle - X[0][0]);
        X[1][0] += K[1][0]*(newAngle - X[0][0]);

     // 协方差更新
        P[0][0] += -K[0][0]*P[0][0];
        P[0][1] += -K[0][0]*P[0][1];
        P[1][0] += -K[1][0]*P[0][0];
        P[1][1] += -K[1][0]*P[0][1];

    }
    /*
     * 一阶互补滤波
     * 角度、角速度
     */
    void cf_firstOrder(float K, float angle_m, float gyro_m)
    {
        angle = K*angle_m + (1-K)*(angle + gyro_m * dt);	// x_2 = x_1 + v*t , t:系统采样时间
    }
 };