卡尔曼滤波

卡尔曼滤波

我们可以有多种手段采集得到数据，传感器采集温度、GPS定位、压力传感器采集压力等等，但是传感器采集得到的数据只是“采集数据”，并不一定是真实数据。

算法原理

对于一个时间序列的数据，假设t时刻的真实数据为$X(t)$，测量数据为$X_m(t)$。那么
$$X_m(t)=X(t)+{\xi }_{1,t}\text{\hspace{1em}(1)}$$
当${\xi }_t$的方差较大时，尤其是当传感器数据发生漂移时，测量数据对真实状态的估计效果不佳。为了提升对真实状态的估计效果。基于原理，假设t时刻的真实数据可以由t-1时刻的真实数据计算得到，即
$$X(t)=f(X(t-1))+{\xi }_{2,t}\text{\hspace{1em}(2)}$$
方程（1）和方程（2）都可以用于估计真实状态。自然的，可以结合方程（1）和方程（2）来的估计结果来得到最终的估计结果。

为了优化计算，我们现在做出假设：

• (a) ${\xi }_{1,t}$的期望为0，方差为${\Sigma }_{1,t}$
• (b) ${\xi }_{2,t}$的期望为0，方差为${\Sigma }_{2,t}$
  为了简化方程，我们假设f(X(t-1))为线性的，且假设：
• (c) $f(X(t-1))=AX(t-1)+Bu(t-1)$

也就是
$$X(t)=AX(t-1)+Bu(t-1)+{\xi }_{2,t}\text{\hspace{1em}(3)}$$
其中u(t-1)为偏置项。

现在，再次回看整个估计过程。我们的目的是为了估计真实的状态序列X(t)，但是我们实际只能采集得到测量序列$X_m(t)$，以及经验方程（1）、（2）。

假设t-1时刻关于真实状态$X(t-1)$的最优估计为$X^{\ast }(t-1)$。令
$$X_p(t)=A{X}^{\ast }(t-1)+Bu(t-1)=A(X^{\ast }(t-1)-X(t-1))+AX(t-1)+Bu(t-1)=A(X^{\ast }(t-1)-X(t-1))+X(t)-{\xi }_{2,t}\text{\hspace{1em}(4)}$$

由假设(a)可知$X_m(t)$是真实状态的无偏估计，假设：

• (d) $X^{\ast }(t-1)$为$X(t-1)$的无偏估计

则$X_p(t)$为真实状态$X(t)$的无偏估计。

不妨设$X^{\ast }(t-1)$方差为${\Sigma }^{\ast }(t-1)$。假设：

• (e) $X^{\ast }(t-1)$与${\xi }_{2,t}$相互独立

则$X_p(t)$的方差满足
$$var(X_p(t))=A{\Sigma }^{\ast }(t-1)A^T+{\Sigma }_{2,t}\text{\hspace{1em}(5)}$$

即$X_p(t)$的期望为0，方差为${\Sigma }_{+,t}=A{\Sigma }^{\ast }(t-1)A^T+{\Sigma }_{2,t}$。

至此，我们得到了真实状态$X(t)$的两个无偏估计$X_p(t)$,$X_m(t)$。现在有两个思路来解决思考这个问题。

思路一

现在假设$X^{\ast }(t)=g(X_p(t),X_m(t))$，为了简化计算，做出如下线性假设：

• (f) $g(X_p(t),X_m(t))=X_p(t)+\lambda (t)\ast (X_m(t)-X_p(t))$

即
$$X^{\ast }(t)=X_p(t)+\lambda (t)(X_m(t)-X_p(t))\text{\hspace{1em}(6)}$$
将（1）、（4）带入公式（6），得到
$$X^{\ast }(t)=(I-\lambda (t))X(t)+\lambda (t)X(t)+(I-\lambda (t)){\xi }_t^{+}+\lambda (t){\xi }_{1,t}=X(t)+(I-\lambda (t)){\xi }_t^{+}+\lambda (t){\xi }_{1,t}$$
估计$X^{\ast }(t)$的协方差为
$$h(\lambda (t))=var(X^{\ast }(t))=E(X^{\ast }(t)-E(X^{\ast }(t))(X^{\ast }(t)-E(X^{\ast }(t)){)}^T\text{\hspace{1em}(7)}$$
为使$X^{\ast }(t)$为最优估计，即使协方差阵的对角线元素之和最小。即求
$$tr(h(\lambda (t)))=tr(E(X^{\ast }(t)-E(X^{\ast }(t))(X^{\ast }(t)-E(X^{\ast }(t)){)}^T)\text{\hspace{1em}(7’)}$$
的最小值。令${\Sigma }_{1,+}(t)$为${\xi }_{1,t}$与${\xi }_t^{+}$的协方差。假设：

• (g) ${\Sigma }_{1,+}(t)=0$

公式（7’）关于$\lambda (t)$求偏导，并另其等于0，得到：
$$-2{\Sigma }_{+,t}+2\lambda (t){\Sigma }_{+,t}+2\lambda (t){\Sigma }_1(t)=0\text{\hspace{1em}(8)}$$

则由公式(7)可以得到：
$$\lambda (t)={\Sigma }_{+,t}({\Sigma }_{+,t}+{\Sigma }_1(t){)}^{-1}\text{\hspace{1em}(9)}$$
其中$(({\Sigma }_{+,t}+{\Sigma }_1(t){)}^{-1}$表示${\Sigma }_{+,t}(t)+{\Sigma }_1(t)$的逆。
将公式(9)带入（6）得到$X^{\ast }(t)$的更新值，带入公式(7)可以得到$X^{\ast }(t)$的方差的
$$var(X^{\ast }(t))={\Sigma }_{+,t}-\lambda (t){\Sigma }_{+,t}\text{\hspace{1em}(10)}$$

思路二

对于真实状态$X(t)$的两个无偏估计$X_p(t)$,$X_m(t)$，方差分别为${\Sigma }_{+,t}$,${\Sigma }_{1,t}$。假设$X_p(t)$、$X_m(t)$都服从正态分布，假设$X_p(t)$和$X_m(t)$相互独立。那么似然函数为
$$L(X(t)|X_p(t),X_m(t))=C{e}^{-1/2\ast (X_p(t)-X(t){)}^T{\Sigma }_{+,t}(X_p(t)-X(t))-1/2\ast (X_m(t)-X(t){)}^T{\Sigma }_{1,t}(X_m(t)-X(t))}\text{\hspace{1em}(11)}$$
对似然函数取对数，关于$X(t)$求偏导，令偏导等于0，求解得到
$$X(t)=({\Sigma }_{+,t}^{-1}+{\Sigma }_{1,t}^{-1}{)}^{-1}({\Sigma }_{+,t}^{-1}{X}_p(t)+{\Sigma }_{1,t}^{-1}{X}_m(t))=({\Sigma }_{+,t}^{-1}+{\Sigma }_{1,t}^{-1}{)}^{-1}{\Sigma }_{+,t}^{-1}{X}_p(t)+({\Sigma }_{+,t}^{-1}+{\Sigma }_{1,t}^{-1}{)}^{-1}{\Sigma }_{1,t}^{-1}{X}_m(t)$$
因为
$${\Sigma }_{+,t}^{-1}+{\Sigma }_{1,t}^{-1}={\Sigma }_{+,t}^{-1}({\Sigma }_{+,t}+{\Sigma }_{1,t}){\Sigma }_{1,t}^{-1}={\Sigma }_{1,t}^{-1}({\Sigma }_{+,t}+{\Sigma }_{1,t}){\Sigma }_{+,t}^{-1}$$
所以
$$X(t)={\Sigma }_{1,t}({\Sigma }_{+,t}+{\Sigma }_{1,t}{)}^{-1}{X}_p(t)+{\Sigma }_{+,t}({\Sigma }_{+,t}+{\Sigma }_{1,t}{)}^{-1}{X}_m(t)\text{\hspace{1em}(12)}$$
取$X^{\ast }(t)={\Sigma }_{1,t}({\Sigma }_{+,t}+{\Sigma }_{1,t}{)}^{-1}{X}_p(t)+{\Sigma }_{+,t}({\Sigma }_{+,t}+{\Sigma }_{1,t}{)}^{-1}{X}_m(t)$为$X(t)$的最优估计。令
$$\lambda (t)={\Sigma }_{+,t}({\Sigma }_{+,t}+{\Sigma }_1(t){)}^{-1}$$
即可得到思路一中的公式(6)和公式(10)

算法小结

假设方程

$$X_m(t)=X(t)+{\xi }_{1,t}$$
$$X(t)=AX(t-1)+Bu(t-1)+{\xi }_{2,t}$$

相关的更新方程

$$X_p(t)=A{X}^{\ast }(t-1)+Bu(t-1)$$
$${\Sigma }_{+,t}=var(X_p(t))=A{\Sigma }^{\ast }(t-1)A^T+{\Sigma }_{2,t}$$
$$X^{\ast }(t)=X_p(t)+\lambda (t)(X_m(t)-X_p(t))$$
$${\Sigma }^{\ast }(t)={\Sigma }_{+,t}-\lambda (t){\Sigma }_{+,t}$$
$$\lambda (t)={\Sigma }_{+,t}({\Sigma }_{+,t}+{\Sigma }_1(t){)}^{-1}$$

算法回顾

卡尔曼滤波中有四条数据序列

• 客观真实的数据序列$X(t)$，无法得到，是我们需要估计的
• 测量序列$X_m(t)$，通过传感器采集得到的，$e_m(t)=X_m(t)-X(t)$的分布由采集设备决定
• 原理预测序列$X_p(t)$，原理本身是基于真实值的方程。因为真实值无法得到，所以$X_p(t)$只能是基于预测值$X^{\ast }(t-1)$得到。所以
  $$e_p(t)=X_p(t)-X(t)=f(X^{\ast }(t-1))-X(t)=f(X^{\ast }(t-1)-X(t-1)+X(t-1))-X(t)$$
  当f为线性变换时
  $$e_p(t)=f(X^{\ast }(t-1)-X(t-1))+f(X(t-1))-X(t)$$
  也就是说$e_p(t)$由$f(X^{\ast }(t-1)-X(t-1))$和$f(X(t-1))-X(t)$两部分得到。
• 最终的预测序列，也就是滤波序列或修正序列$X^{\ast }(t)$，$X^{\ast }(t)=g(X_p(t),X_m(t))$,$e^{\ast }(t)=X^{\ast }(t)-X(t)$

回归之前出现的所有假设：

1. 假设(a)不成立，测量值不是真实值的无偏估计。此时需要假设${\xi }_{1,t}$的期望是个常数，然后通过添加偏置项来得到$X_m(t)$，否则无法计算。
2. 假设(b)不成立，此时可以通过添加偏置项来使得(b)成立，这也是$Bu(t-1)$的作用。当$Bu(t-1)$添加的不合理时，$X_p(t)$不是真实值$X(t)$的无偏估计，那么最终的滤波结果也不是真实值的无偏估计，滤波并没有效果。不过换一个角度，可以以此作为时间序列的异常诊断依据。
3. 假设©不成立，模型难以迭代计算。需要有新的函数f。
4. 假设(d)不成立，那么$X_p(t)$也不是真实值的无偏估计。根据迭代公式，当$A^n$不收敛的时候，$X_p(t)-X(t)$的可能没有界。所以必须控制$X^{\ast }(t-1)-X(t-1)$的期望。
5. 假设(e)的目的是为了更新方差，在假设(e)成立的情况下，方差才能准确计算。换个角度，当假设(e)出现问题的时候，方差的计算结果是有误的，但是如果这个错误的程度是较小的，那也可以计算，假设条件不成立对结果的影响可能是可控的。
6. 假设(f)不成立，这很正常，只是在两个随机变量都服从正态分布时，(f)的效果最好。在不是正态分布时，并不一定是最好的。但是在给定数据条件下，假设(f)成立的情况下得到的滤波总是有一定效果的。
7. 假设(g)几乎不可能不成立。

新息探讨

新息：测量值与预测值之差。

当所有假设都成立的时候，新息的期望为0。虽然t时刻的新息和t+1时刻的新息的方差是不同的，但是新息还是一条在0附近抖动的序列。

当假设(b)不成立：

• 若${\xi }_{2,t}$为一恒定的大于0的常数$\mu 1$，此时t时刻的新息的期望为$\mu 1$，t+1时刻的新息的期望为$\mu 1+\Delta \mu$，其中$\Delta \mu$会小于$\mu 1$

参考文献

• http://www.doc88.com/p-2008731504229.html
• https://blog.youkuaiyun.com/qq_31589695/article/details/80330126