滤波器 状态估计
之前在看文章的时候偶然看到了这个东西，原文中使用了卡尔曼滤波器来估计系统目前的状态，所以就想了解一下这是个什么东西；然后又为什么叫卡尔曼滤波器，‘滤波器’这个词在其中的意义又是什么？

What is Kalman Filter

鲁道夫卡尔曼 递归 离散 系统状态
1960年，鲁道夫.卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问题的论文。经过了几十年的发展，卡尔曼滤波器广泛的应用在了自主或者协助导航领域(大部分学习控制系统的人都知道卡尔曼滤波器吧)。
卡尔曼滤波器能够仅根据上一次的系统状态估计将来的状态，并且提供一种高效的递归方法来不断的优化估计的准确性（通过使得估计均方差最小）,并且即使在不知道物体的实际状况并不确切符合模型的时候也能提供良好的估计结果。

卡尔曼滤波器使用以下公式估计系统实际状态\(x_k, x\in{R^n}\)，以离散模型为例:
\(x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}..........(1.1)\)
\(z_k=Hx_k+v_k.........(1.2)\)
\(A_{n*n}\)表示从第\(k-1\)步到第\(k\)步的状态转移矩阵,在这里\(A\)是可能随着时间变化的;\(B_{n*l}\)表示对于控制向量\(u\in{R^l}\)的增益矩阵;\(H_{m*n}\)表示测量时的转移矩阵;\(w_k v_k\)为过程和测量噪音，模型中假定它们相互独立且分别服从方差为\(Q\)和\(R\)的高斯分布,\(z_k\)即为测量状态;

How it works?

模型系数 预测 预估过程(时间更新) 校正过程(测量更新)
作为一个数据其实不是很好的人，也可以理解上述的表达式，但是想要弄明白上述模型，还需要解决以下几个问题:

  1. 系统的初始状态，即系数如何确定?
  2. 模型如何预测
  3. 如何进行递归优化，要知道现实中测量值往往是包含噪音的，例如雷达，那么如何根据测量值估计更加精确的值呢?
  4. 模型的整个过程
    ###初始状态与系统参数 系数 ###如何预测 预测 预测值根据式1.1直接得到\(\hat{x}^{-}_{k}\),我们称之为先验估计;
    \(\hat{x}^{-}_{k}=A\hat{x}_{k-1}+Bu_{k-1}........(2.1)\),同时引入后验估计\(\hat{x}_{k}\)
    可以看出第\(k\)步的先验估计由第\(k-1\)步的后验估计得到,然后是整个模型的关键等式:
    \(\hat{x}_{k}=\hat{x}^{-}_{k} + K(z_k - H\hat{x}^{-}_{k}).........(2.2)\), 即把先验估计和观测值与估计观测值\(H\hat{x}^{-}_{k}\)的差的线性组合作为后验估计，那么\(K_{n*m}\)怎么来呢？这里引入
    \(e_k=\hat{x}_k - x_k.........(2.3)\) 为后验估计误差,同样有对应的先验估计误差\(e_k^-\)
    \(P_k=E[e_ke_k^T].........(2.4)\),后验估计误差的协方差(同样存在对应的\(P_k^-\)),然后\(K\)使得\(P_k\)最小
    具体来说\(K\)的求法为将\(2.2\)式代入\(2.4\)然后对\(K\)求导，使一阶导数为\(0\),求得\(K\),在本例中
    \(K_k=P_k^-H^T(HP^-_kH^T+R)^{-1}.........(2.5)\)
    ###更新过程 更新 为了使得模型可以继续递归的迭代下去, 我们需要知道几个值\(\hat{x}^{-}_{k-1}\)和\(P^-_{k}\),用于根据\(2.2\)求得\(\hat{x}_k\)和\(P_k\),具体的有:
    \(\hat{x}^{-}_{k}=A\hat{x}_{k-1}+Bu_{k-1}........(2.1)\)
    \(P_k^-=AP_{k-1}A^T+Q.........(2.6)\)

\(K_k=P_k^-H^T(HP^-_kH^T+R)^{-1}.........(2.5)\)
\(\hat{x}_{k}=\hat{x}^{-}_{k} + K(z_k - H\hat{x}^{-}_{k}).........(2.2)\)
\(P_k=(I-K_kH)P_k^-.........(2.7)\)

第一部分称之为时间更新过程，即预测过程，第二部分称之为测量更新过程，即修正过程
需要注意的是，\(\hat{x}_{k-1}^-\)和\(P_{k-2}\)需要设置初始值

整个过程

\(initial.\hat{x}_{k-1}^- , P_{k-2}\) \(\Rightarrow\) \(\hat{x}_{k-1}^- , P_{k-1}^-\) \(\Rightarrow\) \(K_{k-1},\hat{x}_{k-1},P_{k-1}\) \(\Rightarrow\) \(\hat{x}^{-}_{k},P_k^-\) \(\Rightarrow\) \(K_k,\hat{x}_{k},P_k\) \(\Rightarrow\) \(...\)

Why it works？

解释为什么这有用，即需要解释等式\(2.2\)的合理性.
解释来源于贝叶斯规则： \(x_k\) 的更新取决于在已知先前的测量变量\(z_k\)的情况下的先验估计\(\hat{x}^-_k\) 的概率分布.卡尔曼滤波器表达式中包含 了状态分布的前二阶矩.
\(E[x_k]=\hat{x}_k\)
\(E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]=P_k\)
后验状态估计\(2.2\)式反应了状态分布的均值(一阶矩)――如果\(w_kv_k\)服从正态分布的条件成立，均值的估计便是正态分布的。后验估计误差协方差\(2.4\)式反映了状态分布的方差(二阶非中心矩)。在已知\(z_k\)的情况下,\(x_k\)的分布可写为:
\(p(x_k|z_k) \sim N(E[x_k],E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T])\)
\(=N(\hat{x}_k,P_k)\)

Problems i met before

1.为什么叫做滤波器

滤波器特性体现在估计值要比观测值平滑的多

2.等式2.5的推导

unkown

3.等式2.5的意义

等式2.5也可以写成以下形式:
\(K_k=\frac{P^-_kH^T}{HP^-_kH^T+R}\)
可以看见当\(R\)趋于0的时候\(K\)接近于\(H^{-1}\)，此时后验估计更接近于观测值；反之当\(P_k^-\)接近于0时\(K\)也接近0，此时后验估计更接近于估计值

4.等式2.6的推导

\(P_k^- = E[(x_k-\hat{x}_k^-)(x_k-\hat{x}_k^-)^T]\)
由于有:
\(x_k=Ax_{k-1}+Bu_{k-1}+w_k\)
\(\hat{x}_k^-=A\hat{x}_{k-1}+Bu_{k-1}\)
因此
\(x_k-\hat{x}_k^- = Ax_{k-1} - A\hat{x}_{k-1}+w_k\)
那么
\(P_k^- = E[(Ax_{k-1} - A\hat{x}_{k-1}+w_k)(Ax_{k-1} - A\hat{x}_{k-1}+w_k)^T]\)
\(=Cov(Ax_{k-1} - A\hat{x}_{k-1}+w_k, Ax_{k-1} - A\hat{x}_{k-1}+w_k)\)
\(=Var(Ax_{k-1} - A\hat{x}_{k-1}+w_k)\)
\(=Var(Ax_{k-1} - A\hat{x}_{k-1}) + Var(w_k) + 2Cov(Ax_{k-1} - A\hat{x}_{k-1},w_k)\)
\(=Var(Ax_{k-1} - A\hat{x}_{k-1}) + Var(w_k)\)
\(=Var(Ax_{k-1} - A\hat{x}_{k-1}) + Q\)
\(=Cov(Ax_{k-1} - A\hat{x}_{k-1},Ax_{k-1} - A\hat{x}_{k-1}) + Q\)
\(=E[(Ax_{k-1} - A\hat{x}_{k-1})(Ax_{k-1} - A\hat{x}_{k-1})^T] + Q\)
\(=AE[(x_{k-1} - \hat{x}_{k-1})(x_{k-1} - \hat{x}_{k-1})^T]A^T + Q\)
\(=AP_{k-1}A^T + Q\)

5.等式2.7的推导

unkown

参考资料

kalman_introduction.pdf
kalman_introduction_chinese.download
卡尔曼滤波.wiki
simple_implementation.matlab
other_resources.html