卡尔曼滤波的公式详解

1970年01月01日算法

卡尔曼滤波的公式详解

对于初学者来说，卡尔曼滤波一堆的数理公式让人十分头大
这里我们来对卡尔曼公式做出一个具体的解释

首先我们来看看基本卡尔曼的五个公式

$x_{t|t-1} = Ax_{t-1|t-1} + B $
$P_{t|t-1} = AP_{t-1|t-1}A^T + Q_t $
$K_t = P_{t|t-1}H^T(HP_{t|t-1}H^T + R_t)^{-1} $
$x_{t|t} = K_tz_t + (E - K_tH)x_{t|t-1}$
$P_{t|t} = (E-K_tH)P_{t|t-1}$

接下来我来仔细讲解各个公式的意义

卡尔曼滤波你可以理解为带有预测功能的平滑滤波器

在进行滤波时,考虑估计值与测量值得到计算值

我们要做的就是计算估计值与测量值之间的占比关系

首先是第一个公式

卡尔曼滤波要求

我们要根据某一规则得到当前时刻的预测值$\hat{x}^-$

根据普遍规律我们一般根据上一时刻的数据去预测当前时刻的数据$\hat{x}^- = x_{t|t-1}$

则当前根据上一时刻的预测值我们可以描述为
$x_{t|t-1} = Ax_{t-1|t-1} + B$

其中$x_{t-1|t-1}$表示上一时刻滤波得到的计算值
$B$代表当前时刻的外加因素

比如某一汽车以加速度$a$做匀加速运动,$t-1$时刻汽车的速度为$v_{t-1}$

则我们可以预测t时刻汽车的速度为
$v_{t} = v_{t-1} + a,A=1,B=a$

对于有多个预测值，则是采用矩阵运算的方法，那么就可以得到卡尔曼滤波第一个公式
$x_{t|t-1} = Ax_{t-1|t-1} + B $
其中$A$被称为状态转移矩阵，表示从$t-1$时刻的状态转到$t$时刻的状态
$B$则称为控制矩阵，表示t时刻外界的控制量

第二个公式

我们知道，预测值不能表示真实值，因为真实过程当中总会有不同的干扰量

$t$时刻的真实值我们可以用下面的公式描述
$x_t = Ax_{t-1} + B + w_t$
其中$x_t$表示t时刻的真实值,$w_t$表示外部噪声
这里我们先假设噪声服从均值为0的正态分布$w_t \sim N(0,\sigma^2)$
其协方差$cov(w_t)=Q_t$

则我们要描述理论估计值与真实值的误差有多少
我们使用协方差矩阵$P_{t|t-1}$来表示
$P_{t|t-1} = cov(x_t-x_{t|t-1})$
推导得
$P_{t|t-1} = cov(x_t-x_{t|t-1})$
$=cov(Ax_{t-1} + B + w_t - (Ax_{t-1|t-1}) + B )$
$=cov(A(x_{t-1}-x_{t-1|t-1})+w_t)$
$=Acov(x_{t-1}-x_{t-1|t-1})A^T + cov(w_t)$
$=AP_{t-1|t-1}A^T + Q_t$

这样我们就得到了当前$t$时刻的理论估计值误差
对于上面汽车的例子，假如汽车的速度误差服从$w_t \sim N(0,0.05)$
其中A=1
则汽车速度的估计误差可以表示为
$P_{t|t-1}=1 \times P_{t-1|t-1} \times 1 + 0.05$

对于多个估计误差则是卡尔曼第二个公式的矩阵形式

第三个公式

既然我们得到了预测值与预测误差

那我们就要根据预测值与传感器的测量值综合评价，得到滤波后的计算值

首先传感器的测量值肯定不是直接可以利用的数据

比如汽车的速度，传感器可能通过加速度的积分来间接得到速度

因此测量值可以描述为

$z_t = Hx_t + v_t$

其中$H$为变换矩阵,用于将真实值映射到测量值对应的空间来
比如$x_t$为真实速度,$z_t$为测量的加速度,则需要将速度映射到加速度对应的空间来
$v_t$用来表示测量误差，假设$v_t$服从均值为0的正态分布$v_t\sim N(0,\sigma^2)$
其协方差为$cov(v_t) = R_t$

假如$z_t$测量值近似等于真实值，则$H$可以设为1(矩阵形式则为E)

现在我们知道了测量值与真实值的关系

那么和第二个公式一样，我们也需要知道测量值与与估计值之间的误差
我们使用协方差矩阵$S_t$来表示，则
$S_t = cov(z_t - Hx_{t|t-1})$

推导得
$S_t = cov(z_t - Hx_{t|t-1})$
$= cov(Hx_t + v_t - Hx_{t|t-1})$
$= cov(H(x_t-x_{t|t-1}H^T)) + cov(v_t)$
$=HP_{t|t-1}H^T + R_t$

有了估计值的估计误差$P_{t|t-1}$与测量值的测量误差$S_t$
对于上面汽车的例子，假如测量误差$v_t \sim N(0,0.05)$,则$R_t=0.05$

我们就可以描述估计值与测量值之间的关系，接下来我们就要对估计值与测量值进行融合

我们肯定是希望误差越小的一边占比越大，误差越大的一边占比越小

那么我们可以定义
$K_t = P_{t|t-1}H^TS^{-1}_t$

来描述估计值与测量值之间的占比关系，这里$K_t$被称为卡尔曼增益
化简得
$K_t = P_{t|t-1}H^T(HP_{t|t-1}H^T + R_t)^{-1}$

即卡尔曼第三个公式

第四个公式

我们现在知道了估计值与测量值之间的占比关系

那么我们就能根据这个关系进行融合得到滤波后的计算值,则

$x_{t|t} = K_tz_t + (E-K_tH)x_{t|t-1}$
其中$E$是单位矩阵，当估计值与测量值只有一个时，用1表示
$K_t$即表示测量值的占比，相对的$1-K_tH$表示估计值的占比

即卡尔曼第四个公式

第五个公式

得到滤波后的计算值后，我们还要继续更新估计值的误差
我们知道估计值占比$1-K_tH$
那么新的估计值误差也应更新，即

$P_{t|t} = (1-K_TH)P_{t|t-1}$

这样我们就完成了一次卡尔曼滤波循环

对于单传感器滤波，上式可转换为代数式

python测试代码如下

import math
import numpy as np
import matplotlib.pyplot as plt
#卡尔曼滤波

#假设某一温度传感器的波形服从对数分布
X= np.linspace(0,100,100)
wave_orig = np.log(X+1)
wave = wave_orig + np.random.normal(0,0.05,100)#添加噪点


#滤波后的波形
wave_filtered = np.zeros(100)
wave_filtered[0] = wave[0]
P_11 = 0.5#初始预测误差
for i in range(1,100):
    #理论预测
    x_21 = wave_filtered[i-1] + math.log(i+1) - math.log(i)
    #理论估计误差
    P_21 = P_11 + 0.05
    #实际测量值
    z_2 = wave[i]
    #理论估计值与测量值的误差
    S_2 = P_21 + 0.05
    #计算卡尔曼增益
    K_2 = P_21 / S_2
    wave_filtered[i] = K_2 * z_2 + (1-K_2) * x_21
    P_11 = (1-K_2) * P_21