一维卡尔曼滤波器原理及应用（含C代码）

本文介绍一维卡尔曼滤波器原理及应用并用C代码实现。

1.基本概念

卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。

简单讲，卡尔曼滤波器既考虑测量值也考虑预测值（根据系统前一次的状态），来决定输出，因此相比传统的滤波器更加准确和可靠。

2.适用范围

卡尔曼滤波适用于线性高斯系统，即该系统必须是线性的，而且噪声服从正态分布（高斯分布）。更详细一些，噪声通常被建模为一个均值为0，方差为常数的正态分布，也就是高斯分布。该正态分布的横坐标是随机变量的取值，纵坐标是对应取值的概率密度。

3.建模

这里的建模是为系统建模，通过建模我们可以清楚的了解，系统是否可以通过卡尔曼滤波器进行处理，模型如下：

其中，

1)xk为k时刻信号值，xk-1为k-1时刻（前一次）信号值

2)uk为控制量，简单应用场景下一般为0

3)A,B,H是与系统有关的矩阵，一般为常数

4)wk为符合高斯分布的过程噪声，其协方差为Q

5)vk为符合高斯分布的测量噪声，其协方差为R

6)zk为测量值

公式1告诉我们信号值xk是前一次信号值xk-1的线性组合加上控制信号及过程噪声，其过程噪声要求符合高斯分布。

公式2告诉我们测量值是信号值的线性组合加上测量噪声，其测量噪声要求符合高斯分布。

通过对系统建模，我们了解系统是否可以通过卡尔曼滤波器进行处理。

4.公式

本文不介绍相关公式的推导，仅列出推导出的结论。想了解其推导过程可参阅其他资料。

其中，

Time Update(prediction)公式：

1) 为根据k-1时刻的值预测k时刻的结果，也写作Xk|k-1

2)为k-1时刻最优值，也写作Xk-1|k-1

3)A为状态下的变换矩阵

4)B为作用在控制量上的变换矩阵

5)uk为控制量，一般情况下为0

6)为根据k-1时刻的值预测k时刻系统协方差矩阵，也写作Pk|k-1

7)为k-1时刻系统协方差矩阵，，也写作Pk-1|k-1

8)Q为系统过程噪声的协方差

Measurement Update(correction)公式：

1)Kk为卡尔曼增益

2)H为对象的预测矩阵，当系统是一维的时候，H=1

3)R为对象测量噪声的协方差矩阵

4)zk为测量值，也就是输入值

5)为k时刻最优值，也就是输出值

A，B，H大多数情况下，它们为常数，系统为一维情况下为1，对于一个卡尔曼滤波器我们需要确定以下参数（难点）：

1)R，初始值可根据传感器测量误差进行选择

2)Q，初始值可根据实际调试确定

3)x0，初始值一般可设置为0

4)P0，初始值一般可设置为1，P0=0表示完全相信测量值，P0=1表示完全相信估计值，一般是不能设置为0，否则Pk就一直为0了，可设置为1，在后续迭代的过程中，Pk会不断被修正

参数调节的一般方法：

根据公式，我们可以分析得出：R越大，Kk越小；Q越大，Kk越大。Kk越大我们越相信测量值，当我们使用的传感器比较精密，测量噪声很小时，我们越相信测量值，此时应该调小R，反之则调大，另外，当我们的系统模型越准确时，我们的预测值会越精确，此时我们越信任预测值，则应该调小Q。

5.迭代

与传统的数字滤波器（FIR，IIR）滤波器一样，卡尔曼滤波器也是采用迭代的方法进行计算。其迭代过程如下图所示。

迭代过程：

1)Time Update(prediction)。根据前一次状态预估下一时刻状态（输出值，错误协方差）

2)Measurement Update(correction)。计算卡尔曼增益，根据测量值（zk）及预估状态，计算当前最优估计值（输出值）

3)将当前状态赋予下一次状态（输出值，错误协方差），便于下一次计算

6.代码

一维的情况下各矩阵（A，H，I）为1，uk为0，要简单许多，直接套用公式即可，这里为了方便描述直接用float数据，如果MCU计算能力有限，也可以用整型数据进行计算，C代码如下：

typedef struct _KALMAN_FILTER
{
    float xk_1;  //x(k-1|k-1)
    float Pk_1;  //P(k-1|k-1)
    float xkk_1;  //x(k|k-1)
    float Pkk_1;  //P(k|k-1)
    float Kk;  //Kk
    float xk;  //x(k|k)
    float Pk;  //P(k|k)
    float Q;
    float R;
}KALMAN_FILTER;

static void InitKalmanFilter(KALMAN_FILTER *pKalmanFilter);
static float CalcKalmanFilter(KALMAN_FILTER *pKalmanFilter, float zk);

static void InitKalmanFilter(KALMAN_FILTER *pKalmanFilter)
{
    pKalmanFilter->xk_1 = 0;
    pKalmanFilter->Pk_1 = 1;
    pKalmanFilter->xkk_1 = 0;
    pKalmanFilter->Pkk_1 = 0;
    pKalmanFilter->Kk = 0;
    pKalmanFilter->xk = 0;
    pKalmanFilter->Pk = 0;
    pKalmanFilter->Q = 0;
    pKalmanFilter->R = 0.1f;
}

static float CalcKalmanFilter(KALMAN_FILTER *pKalmanFilter, float zk)
{
    //prediction
    pKalmanFilter->xkk_1 = pKalmanFilter->xk_1;
    pKalmanFilter->Pkk_1 = pKalmanFilter->Pk_1 + pKalmanFilter->Q;

    //correction
    pKalmanFilter->Kk = pKalmanFilter->Pkk_1 / (pKalmanFilter->Pkk_1 + pKalmanFilter->R);
    pKalmanFilter->xk = pKalmanFilter->xkk_1 + pKalmanFilter->Kk * (zk - pKalmanFilter->xkk_1);
    pKalmanFilter->Pk = (1.0f - pKalmanFilter->Kk) * pKalmanFilter->Pkk_1;

    pKalmanFilter->xk_1 = pKalmanFilter->xk;
    pKalmanFilter->Pk_1 = pKalmanFilter->Pk;

    return pKalmanFilter->xk;
}

7.应用

这里自行构造了一组数据，并比较原始数据和经过卡尔曼滤波后的数据。

int main()
{
    KALMAN_FILTER KalmanFilter;
    float rawData[10]= {0.39f, 0.50f, 0.48f, 0.29f, 0.25f, 0.32f, 0.34f, 0.48f, 0.41f, 0.45f};
    float filteredData[10] = {0};
    unsigned int i = 0;

    InitKalmanFilter(&KalmanFilter);

    for (i = 0; i < 10; i++)
    {
        filteredData[i] = CalcKalmanFilter(&KalmanFilter, rawData[i]);
    }

    printf("raw data:\r\n");
    for (i = 0; i < 10; i++)
    {
        printf("%.3f ", (double)rawData[i]);
    }
    printf("\r\n");

    printf("filtered data:\r\n");
    for (i = 0; i < 10; i++)
    {
        printf("%.3f ", (double)filteredData[i]);
    }
    printf("\r\n");

    return 0;
}

8.输出结果

将输出数据复制到Excel中，绘制图像如下。

从图中可以明显看出经过卡尔曼滤波后的数据要比原始数据要平滑，且更符合实际情况。

总结，本文介绍了一维卡尔曼滤波器原理及应用并用C代码实现它。