滤波算法（四）—— 卡尔曼滤波算法

大家好，又见面了，我是你们的朋友全栈君。

一、算法介绍

卡尔曼滤波是一个神奇的滤波算法，应用非常广泛，它是一种结合先验经验、测量更新的状态估计算法。

1、状态估计

首先，对于一个我们关心的物理量，我们假设它符合下面的规律

$x_{k}=ax_{k-1}$

其中， $x_{k}$ 为该物理量本周期的实际值， $x_{k-1}$ 为该物理量上一个周期的实际值，当然这个物理量可能不符合这个规律，我们只是做了一个假设。不同的物理量符合的规律不同，是我们的经验，我们根据这个规律可以预测我们关心的物理量。比如，我们关心的物理量是车速，如果车辆接近匀速运动时，则的取值为1，也就是这个周期与上个周期的速度相同。

下面我们再来看一下这个物理量的测量公式

$z_{k}=x_{k}+v_{k}$

其中， $z_{k}$ 是这个物理量的测量值， $v_{k}$ 是测量噪声。我们对一个物理进行预测，测量是一个必不可少的手段，虽然测量的不一定准，但是在很大程度上体现了物理量的实际值。这个公式体现的就是实际值与测量值的关系。还是以车速为例， $z_{k}$ 是通过车速传感器得到的测量值。

实际中，物理量一般不会像我们上面的公式那样简单，一般我们用下面的公式来表示

$x_{k}=ax_{k-1}+bu_{k}$

其中， $bu_{k}$ 代表了处理噪声，这个噪声是处理模型与实际情况的差异，比如车速，他会受到人为加速、减速、路面不平等外界因素的影响。

卡尔曼滤波的基本思想是综合利用上一次的状态和测量值来对物理量的状态进行预测估计。我们用 $\hat{x_{k}}$ 来表示 $x_{k}$ 的估计值，则有下面的公式

$\hat{x}_{k}=\hat{x}_{k-1}+g_{k}\left ( z_{k}-\hat{x}_{k-1} \right )$

在这个公式中，综合利用了上一个周期的估计值和这个周期的测量值来对 $x_{k}$ 进行估计。其中， $g_{k}$ 叫做卡尔曼增益，这个公式与一阶滤波很相似，只不过卡尔曼增益是会变的，每个周期都会更新，一阶滤波的系数则是固定值。考虑极端的情况来分析增益的作用，当 $g_{k}=0$ 时，增益为0，这时 $\hat{x}_{k}=\hat{x}_{k-1}$ ，这表示我们这个周期的估计值与上个周期是相同的，不信任当前的测量值；当 $g_{k}=1$ 时，增益为1，这时 $\hat{x}_{k}=z_{k}$ ，这表示我们这个周期的估计值与测量值是相同的，不信任上个周期的估计值，在实际应用时， $g_{k}$ 介于0~1之间，它代表了对测量值的信任程度。

2、卡尔曼增益

上面我们通过卡尔曼增益来估计物理量的值，那卡尔曼增益又是如何取值的呢？我们通过下面两个公式来计算并在每个周期进行迭代更新。

$g_{k}=p_{k-1}/\left ( p_{k-1}+r \right )$

$p_{k}=\left ( 1-g_{k} \right )p_{k-1}$

在上述公式中，是测量噪声 $v_{k}$ 的平均值，测量噪声是符合高斯分布的，一般可以从传感器厂商那里获得测量噪声的均值，如果无法获得可以根据采集到的数据给出一个经验值。的大小对最终滤波效果的影响是比较大的。 $p_{k}$ 为本周期的预测误差。我们采用分析卡尔曼增益的方法来分析预测误差的作用，即采用假设极端情况的方法。假设前一次的预测误差 $p_{k-1}=0$ ，根据第一个公式则 $g_{k}=0$ ，根据上面的分析，这种情况估计值为上个周期的估计值；如果前一次的预测误差 $p_{k-1}=1$ ，则增益变为 $1/\left ( 1+r \right )$ ，一般取值很小，所以 $g_{k}\approx 1$ ，这种情况以新测量的值作为估计值。

对于第二个公式，当卡尔曼增益为0时， $p_{k}=p_{k-1}$ ，即采用上一个周期的预测误差；当增益为1时， $p_{k}=0$ 。

3、完整卡尔曼滤波算法

有了上面的推导，我们在下面列出来完成卡尔曼滤波的公式，卡尔曼滤波分为预测过程和更新过程两个过程，在公式中，我们又引入了缩放系数，和协方差。

预测过程：

$\hat{x}_{k}=a\hat{x}_{k-1}+bu_{k}$

$p_{k}=ap_{k-1}a+q$

更新过程：

$g_{k}=p_{k}h/\left ( hp_{k}h+r \right )$

$\hat{x}_{k}=\hat{x}_{k}+g_{k}\left ( z_{k}-h\hat{x}_{k} \right )$

$p_{k}=\left ( 1-g_{k}h \right )p_{k}$

上面的公式适合一维变量的卡尔曼滤波，将变量扩展到多维，用向量和矩阵替换上面的变量，就可以实现多维变量的卡尔曼滤波，下面的公式适用于多维变量。

预测过程：

$\hat{x}_{k}=A\hat{x}_{k-1}+Bu_{k}$

$P_{k}=AP_{k-1}A^{T}+Q$

更新过程：

$G_{k}=P_{k}H^{T}\left ( HP_{k}H^{T}+R \right )^{-1}$

$\hat{x}_{k}=\hat{x}_{k}+G_{k}\left ( z_{k}-H\hat{x}_{k} \right )$

$P_{k}=\left ( 1-G_{k}H \right )P_{k}$

二、实现代码

下面我们通过c++代码来实现卡尔曼滤波算法，所实现的算法为一维滤波算法。首先定义卡尔曼滤波的参数

typedef struct{
    float filterValue;//滤波后的值
    float kalmanGain;//Kalamn增益
    float A;//状态矩阵
    float H;//观测矩阵
    float Q;//状态矩阵的方差
    float R;//观测矩阵的方差
    float P;//预测误差
    float B;
    float u;
}KalmanInfo;

下面是卡尔曼滤波器的初始化函数，在这个函数中，info为卡尔曼滤波参数的指针。初始化的参数是针对一个车速滤波过程的设置。

void Kalm::initKalmanFilter(KalmanInfo *info)
{
    info->A = 1;
    info->H = 1;
    info->P = 0.1;
    info->Q = 0.05;
    info->R = 0.1;
    info->B = 0.1;
    info->u = 0;
    info->filterValue = 0;
}

卡尔曼滤波过程函数，函数的输入info为卡尔曼滤波参数的指针，new_value为新的测量值，函数返回滤波后的估计值。

float Kalm::kalmanFilterFun(KalmanInfo *info, float new_value)
{
    float predictValue = info->A*info->filterValue+info->B*info->u;//计算预测值
    info->P = info->A*info->A*info->P + info->Q;//求协方差
    info->kalmanGain = info->P * info->H /(info->P * info->H * info->H + info->R);//计算卡尔曼增益
    info->filterValue = predictValue + (new_value - predictValue)*info->kalmanGain;//计算输出的值
    info->P = (1 - info->kalmanGain* info->H)*info->P;//更新协方差
    return info->filterValue;
}

三、示例

下面我们通过是一个车速滤波的示例来体验卡尔曼滤波的效果。通过上面的介绍，R对滤波效果的影响比较大，在这个示例中，我们分别将R取为0.1和0.5，来看一下车速的滤波效果。首先R取为0.1时，滤波效果如下图所示。其中，蓝色线为滤波前的车速，红色线为滤波后的车速。从图中可以看到滤波后的信号与滤波前的信号跟随很好，滞后很小。基本波动被滤掉了，但也带入了一些波动。