卡尔曼滤波（Kalman Filter）及其动态仿真

估计问题（Estimation）

首先看下什么是估计问题，现有一信号$x_1(t)$和噪声$x_2(t)$，我们能观测到的是两者之和$$y(t)=x_1(t)+x_2(t)$$假定我们观测到具体的数值$y(t_0)\cdots y(t)$，期望从这些观测数据推测出$t_1$时刻的信号$x_1(t_1)$，$t_1$和$t$时刻的关系有三种

$t_1$和$t$的关系	对应的问题
$t_1<t$	平滑
$t_1=t$	滤波
$t_1>t$	预测

这三类关系对应的问题统称为估计问题。根据量测$y(t_0),y(t_1),\cdots y(t)$对$x_1(t_1)$的估计记为$X_1(t_1|t)$，可以得到条件分布$$\text{Pr}[x_1(t_1)<\xi |y(t_0),y(t_1),\cdots ,y(t)]$$

最小均方误差估计（Minimum Mean Square Error Estimator）

在先前的笔记中介绍了最小均方误差估计（可以翻阅参数估计（Parameter Estimation）），本节将其推广到向量形式。最小均方误差估计值为条件均值$E(x|z)$，其中$x$为待估计量，$z$为观测量。求解条件均值，直接想法是求条件概率密度函数$p(x|z)$。假定$x$和$z$满足联合正态分布，即
$$y=\left[\begin{array}{rl}& x\\ & z\end{array}\right]\sim \mathcal{N}\left\{\right[\begin{array}{rl}& \bar{x}\\ & \bar{z}\end{array}],[\begin{array}{rl}& P_{xx}{P}_{xz}\\ & P_{zx}{P}_{zz}\end{array}\left]\right\}$$其中$\bar{x}$为$x$的均值，$\bar{z}$为$z$的均值，$P_{xx}$和$P_{zz}$为各自方差，$P_{xz}$为协方差。联合变量满足 y ∼ N { y ˉ , P y y } y \sim N\{\bar{y},P_{yy}\} y∼N{yˉ​,Pyy​}，可由贝叶斯公式得到条件概率密度函数$$p(x|z)=\frac{p(x,z)}{p(z)}=\frac{p(y)}{p(z)}$$将概率密度函数代入得到 p ( x ∣ z ) = ∣ 2 π P y y ∣ − 1 / 2 exp { − 1 2 ( y − y ˉ ) ′ P y y − 1 ( y − y ˉ ) } ∣ 2 π P z z ∣ − 1 / 2 exp { − 1 2 ( z − z ˉ ) ′ P z z − 1 ( z − z ˉ ) } p(x|z)=\dfrac{|2\pi P_{yy}|^{-1/2}\text{exp}\{-\dfrac{1}{2}(y-\bar{y})'P_{yy}^{-1}(y-\bar{y})\}}{|2\pi P_{zz}|^{-1/2}\text{exp}\{-\dfrac{1}{2}(z-\bar{z})'P_{zz}^{-1}(z-\bar{z})\}} p(x∣z)=∣2πPzz​∣−1/2exp{−21​(z−zˉ)′Pzz−1​(z−zˉ)}∣2πPyy​∣−1/2exp{−21​(y−yˉ​)′Pyy−1​(y−yˉ​)}​该条件概率密度函数仍遵从高斯形式，通过整理可以得到均值，具体推导步骤见文末附录一$$E[x|z]=\bar{x}+P_{xz}{P}_{zz}^{-1}(z-\bar{z})$$相应的条件协方差为$$P_{xx|z}=P_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}$$可以看到，在该情况下的最小均方误差估计，与被估计量先验均值和量测先验均值有关。在此基础上推导最小均方误差的动态估计，即卡尔曼滤波（Kalman filter）

卡尔曼滤波（Kalman Filter）

状态估计是针对动态系统而言的，状态的演变过程由状态转移方程描述。用$x(k)$表示在$k$时刻下的系统状态，$F$用来描述$k+1$时刻和$k$时刻状态之间的关系，被称为状态转移矩阵。不考虑状态过程噪声时候，状态转移方程可以写作$$x(k+1)=Fx(k)$$区别于静态的参数估计，动态系统的待估计量是按照一定规律进行变化的。观测量和状态之间的关系用转移矩阵$H$来描述$$z(k+1)=Hx(k+1)+w(k+1)$$其中$w(k+1)$为传感器的测量噪声，假定其为零均值，方差为$R(k+1)$的高斯白噪声。我们要估计$k+1$时候的状态$x(k+1)$，根据我们已经获得的量测数据集合 z k + 1 = { z 1 , z 2 , ⋯   , z k + 1 } z^{k+1}=\{z_1,z_2,\cdots,z_{k+1}\} zk+1={z1​,z2​,⋯,zk+1​}。假定在$k$时刻，已经得到了状态最小均方误差估计值$${\hat{x}}_{k|k}=E[x(k)|z^k]$$按照上一节所介绍的静态参数最小均方误差估计，推导Kalman滤波过程

1. 求解$\bar{x}\to$在$k+1$时刻的先验状态均值 $${\hat{x}}_{k+1|k}=E[x(k+1)|z^k]$$代入状态转移方程得到$${\hat{x}}_{k+1|k}=E[Fx(k)|z^k]=F{\hat{x}}_{k|k}$$
2. 求解$\bar{z}\to$在$k+1$时刻的均值$${\hat{z}}_{k+1|k}=E[z(k+1)|z^k]$$ 代入量测方程，因量测噪声为零均值所以有$${\hat{z}}_{k+1|k}=E[Hx(k+1)+w(k+1)|z^k]=H{\hat{x}}_{k+1|k}$$
3. 求解方差和协方差项
   $$\begin{array}{rl}{P}_{xx}& \to P_{k+1|k}=E[{\tilde{x}}_{k+1|k}{\tilde{x}}_{k+1|k}^{\prime }|z^k]\\ & =E[(x(k+1)-{\hat{x}}_{k+1|k})((x(k+1)-{\hat{x}}_{k+1|k}{)}^{\prime }|z^k]\\ & =E[(Fx(k)-F{\hat{x}}_{k|k})(Fx(k)-F{\hat{x}}_{k|k}{)}^{\prime }|z^k]\\ & =E[F{\tilde{x}}_{k|k}{\tilde{x}}_{k|k}^{\prime }{F}^{\prime }|z^k]\\ & =F{P}_{k|k}{F}^{\prime }\end{array}$$ $$\begin{array}{rl}{P}_{xz}& \to E[(x(k+1)-{\hat{x}}_{k+1|k})((z(k+1)-{\hat{z}}_{k+1|k}{)}^{\prime }|z^k]\\ & =E[{\tilde{x}}_{k+1|k}(Hx(k+1)+w(k+1)-H{\hat{x}}_{k+1|k}{)}^{\prime }|z^k]\\ & =E[{\tilde{x}}_{k+1|k}{\tilde{x}}_{k+1|k}^{\prime }{H}^{\prime }|z^k]\\ & =P_{k+1|k}{H}^{\prime }\end{array}$$$$\begin{array}{rl}{P}_{zz}& \to S_{k+1}\\ & =E[(z(k+1)-{\hat{z}}_{k+1|k})((z(k+1)-{\hat{z}}_{k+1|k}{)}^{\prime }|z^k]\\ & =E[(H{\tilde{x}}_{k+1|k}+w(k+1))(H{\tilde{x}}_{k+1|k}+w(k+1){)}^{\prime }|z^k]\\ & =H{P}_{k+1|k}{H}^{\prime }+R(k+1)\end{array}$$
4. 代入最小均方误差估计计算公式$${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+P_{k+1|k}{H}^{\prime }{S}_{k+1}^{-1}(z_{k+1}-{\hat{z}}_{k+1|k})$$$$P_{k+1|k+1}=P_{k+1|k}-P_{k+1|k}{H}^{\prime }{S}_{k+1}^{-1}H{P}_{k+1|k}$$

理解Kalman滤波

卡尔曼滤波做了一件什么样的事呢？实质是两个分布的融合

从信号统计特性去看整个滤波过程会有所发现。现有两种概率分布，一是根据数学建模得到的概率分布，二是量测的概率分布，Kalman滤波器将这两种分布经过最优整合得到估计结果，即输出概率分布的均值。采取的最优便是第二节中提到的线性最小均方误差准则。不难发现，整个滤波过程中只用到了噪声统计特性的一阶矩和二阶矩，因此适用于非平稳的信号估计

在这里推荐Matlab官方讲解Kalman滤波的视频，个人认为其对卡尔曼滤波的讲解较为深刻。

另有一篇IEEE 文献用更为直观的方式推导了Kalman滤波，用到高斯概率密度函数的乘积是高斯分布形式。

Kalman滤波仿真

旨在通过一简单例子，探索下Kalman滤波到底干了啥？

仿真场景：现有一小车沿着水平的道路直线匀速行驶，传感器每隔一段时间$T$便采集下其位置，但传感器并不精确，存在测量噪声。后端采集得到一批带有噪声的位置数据，想通过Kalman滤波提高对小车状态估计精度。这里为了从统计角度分析Kalman滤波过程，进行了1000次的蒙特卡洛仿真。

仿真代码已上传资源（有需求者可点击此下载）

对蒙特卡洛仿真采集到的位置数据绘制直方图，可以看到测量位置满足高斯分布，从量测方程可知分布的均值为该时刻位置

如上一节所说的两个分布的融合，在卡尔曼滤波过程中，绘制了量测的预测和量测动态统计特性图

最后给出均方根误差曲线，表明卡尔曼滤波能有效地进行状态估计

附录一

p ( x ∣ z ) = ∣ 2 π P y y ∣ − 1 / 2 exp { − 1 2 ( y − y ˉ ) ′ P y y − 1 ( y − y ˉ ) } ∣ 2 π P z z ∣ − 1 / 2 exp { − 1 2 ( z − z ˉ ) ′ P z z − 1 ( z − z ˉ ) } = ∣ 2 π P y y ∣ − 1 / 2 ∣ 2 π P z z ∣ − 1 / 2 exp { − 1 2 q } \begin{aligned} p(x|z)&=\dfrac{|2\pi P_{yy}|^{-1/2}\text{exp}\{-\dfrac{1}{2}(y-\bar{y})'P_{yy}^{-1}(y-\bar{y})\}}{|2\pi P_{zz}|^{-1/2}\text{exp}\{-\dfrac{1}{2}(z-\bar{z})'P_{zz}^{-1}(z-\bar{z})\}}\\ &=\dfrac{|2\pi P_{yy}|^{-1/2}}{|2\pi P_{zz}|^{-1/2}}\text{exp}\{-\dfrac{1}{2}q\}\end{aligned} p(x∣z)​=∣2πPzz​∣−1/2exp{−21​(z−zˉ)′Pzz−1​(z−zˉ)}∣2πPyy​∣−1/2exp{−21​(y−yˉ​)′Pyy−1​(y−yˉ​)}​=∣2πPzz​∣−1/2∣2πPyy​∣−1/2​exp{−21​q}​其中$$q=(y-\bar{y}{)}^{\prime }{P}_{yy}^{-1}(y-\bar{y})-(z-\bar{z}{)}^{\prime }{P}_{zz}^{-1}(z-\bar{z})$$我们关心的是指数项里的均值，将其整理为$(v-\bar{v}{)}^{\prime }{P}_{vv}^{-1}(v-\bar{v})$的形式，首先求$P_{yy}^{-1}$
已知$$P_{yy}=\left[\begin{array}{rl}& P_{xx}{P}_{xz}\\ & P_{zx}{P}_{zz}\end{array}\right]$$假设$$P_{yy}^{-1}=\left[\begin{array}{rl}& T_{xx}{T}_{xz}\\ & T_{zx}{T}_{zz}\end{array}\right]$$对分块矩阵求逆得到以下关系$$T_{xx}^{-1}=P_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx},P_{zz}^{-1}=T_{zz}-T_{zx}{T}_{xx}^{-1}{T}_{xz},T_{xx}^{-1}{T}_{xz}=-P_{xz}{P}_{zz}^{-1}$$令$$\xi =z-\bar{z},ϵ=y-\bar{y}=\left[\begin{array}{rl}& \eta \\ & \xi \end{array}\right]$$$q$可整理为$$\begin{array}{rl}q& ={ϵ}^{\prime }{P}_{yy}^{-1}ϵ-{\xi }^{\prime }{P}_{zz}^{-1}\xi \\ & ={\eta }^{\prime }{T}_{xx}\eta +{\xi }^{\prime }{T}_{zx}\eta +{\eta }^{\prime }{T}_{xz}\xi +{\xi }^{\prime }{T}_{zz}\xi -{\xi }^{\prime }{P}_{zz}^{-1}\xi \\ & =(\eta +T_{xx}^{-1}{T}_{xz}\xi {)}^{\prime }{T}_{xx}(\eta +T_{xx}^{-1}{T}_{xz}\xi )+{\xi }^{\prime }(T_{zz}-T_{zx}{T}_{xx}^{-1}{T}_{xz})\xi -{\xi }^{\prime }{P}_{zz}^{-1}\xi \\ & =(\eta +T_{xx}^{-1}{T}_{xz}\xi {)}^{\prime }{T}_{xx}(\eta +T_{xx}^{-1}{T}_{xz}\xi )\end{array}$$其中$$\eta +T_{xx}^{-1}{T}_{xz}\xi =x-\bar{x}-P_{xz}{P}_{zz}^{-1}(z-\bar{z})$$条件均值为$$E[x|z]=\bar{x}+P_{xz}{P}_{zz}^{-1}(z-\bar{z})$$相应的条件方差等于$T_{xx}$$$P_{xx|z}=P_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}$$