卡尔曼滤波概念

卡尔曼滤波器是一种递归的滤波器，用于从一系列包含噪声的测量数据中估计系统的状态。它在许多领域中都有广泛应用，特别是在导航、控制、信号处理等领域。适用于线性高斯系统，即系统中的噪声满足高斯分布。（电子设备的热噪声、测量误差都可以假设符合高斯分布。）
卡尔曼滤波简单说就是根据你更相信模型的预测还是传感器的测量值，设置二者的权重，再把二者结合起来做一个修正，得到一个最优的估计值。

卡尔曼滤波公式

x的角标-表示为先验估计值，不带角标的均为最优估计值。F为状态转移矩阵，描述当前状态与上一个状态的关系。B为控制矩阵，描述输入与状态之间的关系。
该公式根据上一时刻的最优估计值与当前的输入做一个估计。

P为协方差矩阵，带有-均为先验协方差也是一个预测。Q为过程噪声的协方差矩阵。过程噪声是系统演变过程中的不确定性，一般来源是模型参数的不准确。
该公式通过上一时刻的最优估计的协方差，预测一个先验估计值协方差。

K为卡尔曼增益，用来设置估计值和观测值的权重。H为观测矩阵，指的是观测值和状态值的关系。R为观测噪声协方差。观测噪声是在观测系统状态时引入的误差，一般来源是传感器本身的精度。

z为观测值一般为传感器的读数。这个公式我的理解是：最优估计值=先验估计值+K*观测值。而K就是你要选择更相信预测值还是更相信观测值。这取决于你的模型是否完美或者传感器精度是否足够高。
K的值主要是受到R和Q的影响。假设H=F=1那么得到K

增大Q表示过程噪声比较大即系统内部的不确定性较高，卡尔曼增益K也会增大，表示更信任观测值。
若增大R表示观测噪声比较大即观测值不确定性更高，卡尔曼增益减小，表示更相信估计值。

该公式就是更新协方差矩阵，用于下一次预测。
在一维的情况下，不存在矩阵均为常数。在写程序时比较简单。多维的情况就是观测的状态数变多了，且每个状态都有自己的噪声。

代码

#include "DSP2833x_Device.h"
#include "DSP2833x_Examples.h"
typedef struct _Kalman_Filter
{
    float xk;//k时刻最优值
    float xk_1;//k-1时刻最优值
    float xkk_1;//k时刻先验估计值
    float uk;//输入
    float Pk;//k时刻最优值协方差矩阵
    float Pk_1;//k-1时刻最优值的协方差矩阵
    float Pkk_1;//k时刻先验估计值协方差矩阵
    float Q,R;
    float K;
}Kalman_Filter;
#define Kalman_Defaults {0,0,0,0,0,1,0,0,0.1f,0}
float Kalman_Cal(Kalman_Filter *pKalman,float zk)
{
    pKalman->xkk_1=pKalman->xk_1+pKalman->uk;
    pKalman->Pkk_1=pKalman->Pk_1+pKalman->Q;
    pKalman->K=pKalman->Pkk_1/(pKalman->Pkk_1+pKalman->R);
    pKalman->xk=pKalman->xkk_1+pKalman->K*(zk-pKalman->xkk_1);
    pKalman->xk_1=pKalman->xk;
    pKalman->Pk=(1.0f-pKalman->K)*pKalman->Pkk_1;
    pKalman->Pk_1=pKalman->Pk;
    return pKalman->xk;
}
void main(void)
{
    int i;
    Kalman_Filter Kalman=Kalman_Defaults;
    float Data[15]={0.50f,1.20f,1.50f,0.80f,0.95f,0.60f,1.50f,2.00f,0.50f,0.70f,0.60f,0.30f,1.20f,1.60f,0.80f};
    float Filter_Data[15]={0};
    for(i=0;i<15;i++)
    {
        Filter_Data[i]=Kalman_Cal(&Kalman,Data[i]);
    }
}

结果

可以看到经过滤波后，曲线变得更平滑。