概率机器人笔记(3)：卡尔曼滤波的理解与简单推导

1.前言

卡尔曼滤波（Kalman Filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。上面是来自百度百科对卡尔曼滤波的定义，指明了卡尔曼滤波的两大主要功能：过滤噪声和参数估计。

2.卡尔曼滤波的理解和推导

(1)对象模型(Dynamic Model)
对象模型就是卡尔曼滤波适应的问题对象，包括状态转移模型和测量模型，由于引入了高斯噪声，在一定程度上可以模拟现实中的很多问题
$$\{\begin{array}{ll}{x}_t=A{x}_{t-1}+B{u}_t+{\omega }_t,& \text{ωt∼N(0,Qt)}\\ z_t=H{x}_t+{\upsilon }_t,& \text{υt∼N(0,Rt)}\end{array}$$其中，$x_t$是系统的状态，$z_t$是测量结果向量，$A$是状态转移矩阵，$B$是控制矩阵，$H$是变换矩阵，作用是状态变换到测量值的维度上，统一单位。${\omega }_t$和${\upsilon }_t$都服从零均值的高斯分布。

也就是说，在实际模型中，无论是状态(火车速度或者位置)还是传感器的测量(位置传感器)都是存在噪声的，符合我们的认知。
(2)预测模型(Prediction Model)
上述模型由于存在噪声，$x_t$不能直接被观测，而模型中的不确定性通过协方差矩阵$P$来表示。在预测模型中，用${\hat{x}}_t^{-}$表示$t$时刻由预测模型得到的状态。$P$是状态中变量的协方差矩阵，根据对象模型，我们可以得到预测模型。
$$\{\begin{array}{l}{\hat{x}}_t^{-}=A{\hat{x}}_{t-1}^{-}+B{u}_t,预测的均值\\ P_t^{-}=A{P}_{t-1}^{-}{A}^T+Q_t,,预测的协方差\end{array}$$根据文献1的推导，：
$$P_t^{-}=E[(x_t-{\hat{x}}_t^{-})(x_t-{\hat{x}}_t^{-}{)}^T]$$$$P_t^{-}=E[(A(x_{t-1}-{\hat{x}}_{t-1}^{-})+{\omega }_t)(A(x_{t-1}-{\hat{x}}_{t-1}^{-})+{\omega }_t{)}^T]$$$$=AE[(x_{t-1}-{\hat{x}}_{t-1}^{-})(x_{t-1}-{\hat{x}}_{t-1}^{-}{)}^T]A^T+E({\omega }_t{\omega }_t^T)$$$$=A{P}_{t-1}^{-}{A}^T+Q_t$$根据对象模型，由于${\omega }_t$是零均值的高斯分布，当给定状态转移矩阵时，下一时刻分布的均值也就很容易得到了，就是上式的${\hat{x}}_{t-1}^{-}$。比较复杂的是求下一时刻不确定因素的传递，由于我们预测了下一时刻的均值${\hat{x}}_{t-1}^{-}$，预测下一时刻的协方差矩阵自然也是以预测的均值为均值，$P_t^{-}=E[(x_t-{\hat{x}}_t^{-})(x_t-{\hat{x}}_t^{-}{)}^T]$，由于每一次预测都加上$Q_t$，加上运动带来的不确定性m如果不加以修正，预测的不确定性会越来越大，预测函数也会趋于扁平。如下图所示：
(3)测量模型
测量模型由对象模型给出，一般来说，测量数据都是由传感器获得的。
$$z_t=H{x}_t+{\upsilon }_t,{\upsilon }_t\sim N(0,R_t)$$

(4)更新模型(Update Model)
卡尔曼滤波之所以说是最佳的估计，是因为它结合了预测和测量，而实现这一效果就是把预测和测量的概率密度函数相乘。
下面研究一下两个概率密度函数相乘的情况，具体参考文献2。
$$y_1=\frac{1}{\sqrt{2\pi }{\sigma }_1}{e}^{-\frac{(x-{\mu }_1{)}^2}{2{\sigma }_1^2}},y_2=\frac{1}{\sqrt{2\pi }{\sigma }_2}{e}^{-\frac{(x-{\mu }_2{)}^2}{2{\sigma }_2^2}}$$$$y_1{y}_2=\frac{1}{2\pi {\sigma }_1{\sigma }_2}{e}^{-\frac{(x-{\mu }_1{)}^2}{2{\sigma }_1^2}-\frac{(x-{\mu }_2{)}^2}{2{\sigma }_2^2}}$$需要进一步化简成下面的形式$$y_1{y}_2=\frac{1}{\sqrt{2\pi }{\sigma }_{fused}}{e}^{-\frac{(x-{\mu }_{fused}{)}^2}{2{\sigma }_{fused}^2}}$$
具体的推导比较复杂，尤其在归一化部分，但是得到下面的结论还是比较简单的
$${\mu }_{fused}=\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}={\mu }_1+\frac{{\sigma }_1^2({\mu }_2-{\mu }_1)}{{\sigma }_1^2+{\sigma }_2^2}={\mu }_1+K({\mu }_2-{\mu }_1)$$$${\sigma }_{fused}^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}={\sigma }_1^2-\frac{{\sigma }_1^4}{{\sigma }_1^2+{\sigma }_2^2}={\sigma }_1^2-K{\sigma }_1^2$$$$K=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}$$当预测和测量的单位不一致时，需要将预测的单位转变为测量的单位，然后进行融合：$${\mu }_{fused}={\mu }_1+K({\mu }_2-H{\mu }_1)$$$${\sigma }_{fused}^2={\sigma }_1^2-KH{\sigma }_1^2$$$$K=\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}$$令$y_1$为预测模型，$y_2$为测量模型
$$y_1\sim N({\hat{x}}_t^{-},P_t^{-}),({\mu }_1={\hat{x}}_t^{-},{\sigma }_1^2=P_t^{-})$$$$y_2\sim N(z_t,R_t),({\mu }_2=z_t,{\sigma }_2^2=R_t)$$$${\mu }_{fused}={\hat{x}}_t,{\sigma }_{fused}^2=P_t$$带入上式：$$K=\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}=\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}$$$${\hat{x}}_t={\hat{x}}_t^{-}+K(z_t-H{\hat{x}}_t^{-})$$$$P_t=(I-KH)P_k^{-}$$
综上所述，卡尔曼算法已经全部推导出来。

完整的卡尔曼滤波公式：
预测模型：
$${\hat{x}}_t^{-}=A{\hat{x}}_{t-1}^{-}+B{u}_t$$$$P_t^{-}=A{P}_{t-1}^{-}{A}^T+Q_t$$更新模型：$$K=\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}$$$${\hat{x}}_t={\hat{x}}_t^{-}+K(z_t-H{\hat{x}}_t^{-})$$$$P_t=(I-KH)P_k^{-}$$
实例：
对上面火车的例子进行matlab仿真。
对象模型：$$\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]+\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]+Q_t,Q_t\sim N(0,\left[\begin{array}{cc}0.0001& 0\\ 0& 0.0001\end{array}\right])$$
matlab仿真代码：

clc
clear
%*****准备观测值*******%
z=zeros(1,50);
r=zeros(1,50);
figure
for i=1:50
z(i)=0.2*i*i+i*randn();
r(i)=0.2*i*i;
end
t=1:1:50;
plot(t,z,"s")
hold on
plot(t,r,"b-")

x=[0;0];% 初始状态
A=[1 1;0 1];% 状态转移矩阵
P=[1 0;0 1];%状态协方差矩阵
Q=[0.0001,0;0,0.0001];%状态转移的协方差矩阵
H=[1 0];%观测矩阵
R=1;%噪声方差
u=0.01;%加速度1
B=[1/2;1];
xp=zeros(1,50);
xv=zeros(1,50);

xp_=zeros(1,50);
xv_=zeros(1,50);
for i=1:50
    x_=A*x+B*u;
    P_=A*P*A'+Q;
    K=(P_*H')/(H*P_*H'+R);
    x=x_+K*(z(i)-H*x_);
    P=(eye(2)-K*H)*P_;
    xp(i)=x(1);
    xv(i)=x(2);
    xp_(i)=x_(1);
    xv_(i)=x_(2);
end
plot(t,xp,"*-")
plot(t,xp_,"<-")
hold off

仿真结果：

3.概率机器人中的卡尔曼滤波

在概率机器人中作为贝叶斯滤波的一个分支，卡尔曼滤波除了需要遵循马尔可夫假设之外，还要遵循以下三个假设：
(1)状态转移概率$P(x_t|u_t,x_{t-1})$必须是带有随机高斯噪声的连续的线性函数。
$$x_t=A_t{x}_{t-1}+B_t{u}_t+{\epsilon }_t,\epsilon \sim N(0,R_t)$$
(2)测量概率$P(z_t|x_t)$也是带有高斯噪声的线性函数
$$z_t=C_t{x}_t+{\delta }_t,\delta \sim N(0,Q_t)$$
(2)初始状态也服从正态分布
$$P(x_0)\sim N({\mu }_0,{\Sigma }_0)$$

从贝叶斯公式角度理解卡尔曼滤波
也可以说从卡尔曼滤波的角度理解贝叶斯滤波，在前上一篇中我们了解了贝叶斯滤波，利用贝叶斯公式推导下面的公式：
$$P(x_t|z_1,z_2,\cdot \cdot \cdot ,z_t)=\eta P(z_t|x_t)P(x_t|z_1,z_2,\cdot \cdot \cdot ,z_{t-1})$$其实在这里，$P(z_t|x_t)$就是测量模型，$P(x_t|z_1,z_2,\cdot \cdot \cdot ,z_{t-1})$就是预测模型。为了产生递归，同样地将$x_{t-1}$引入预测模型：$$P(x_t|z_1,z_2,\cdot \cdot \cdot ,z_t)=\eta P(z_t|x_t){\int }_{x_{t-1}}P(x_t,x_{t-1}|z_1,z_2,\cdot \cdot \cdot ,z_{t-1})d{x}_{t-1}$$$$=\eta P(z_t|x_t){\int }_{x_{t-1}}P(x_t|x_{t-1},z_1,z_2,\cdot \cdot \cdot ,z_{t-1})P(x_{t-1}|z_1,z_2,\cdot \cdot \cdot ,z_{t-1})d{x}_{t-1}$$$$=\eta P(z_t|x_t){\int }_{x_{t-1}}P(x_t|x_{t-1})P(x_{t-1}|z_1,z_2,\cdot \cdot \cdot ,z_{t-1})d{x}_{t-1}$$$$本时刻的最佳估计=\eta \ast 测量模型\ast 预测模型(状态转移\ast 上一时刻的最佳估计)$$

参考文献：
1.Faragher, Ramsey. Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation [Lecture Notes][J]. IEEE Signal Processing Magazine, 2012, 29(5):128-132.
2.P.A. Bromiley. Products and Convolutions of Gaussian Probability Density
Functions