Kalman and Bayesian Filters in Python 1---- linear kalman filter

1.Introduction

本系列将“https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python”的内容进行整理。
下面这个图是kalman filter 的核心思想，结合系统自身的动力学特性做预测，再结合传感器的数据，进行更新。

变量解释：

Column 1	Column 2
x	状态真实值
$\hat{x}$	状态的estimate (posterior)
$\overline{x}$	状态的预测值 (prior)
$z_k$	传感器的数值 (measurement)

2. gh filter

• prediction
  $$\begin{array}{rl}\overline{x}& =\hat{x}+\hat{v}\ast dt\\ \overline{v}& =\hat{v}\end{array}\text{\hspace{1em}(2.1)}$$
• update
  x和v的更新，都可以用下面这个图表示。
  
  $$\begin{array}{rlr}\hat{x}& =\overline{x}+g(z_k-\overline{x})& \\ \hat{v}& =\overline{v}+h\frac{(z_k-\hat{x})}{\Delta t}& \end{array}\text{\hspace{1em}(2.2)}$$
  g和h的取值范围是[0,1]代表着我们对测量数据的相信程度，越相信测量数据，则g和h越大。

3. 离散贝叶斯滤波器

离散贝叶斯滤波器同样遵循gh filter 的框架。

• prediction
  作者以tracking dog为例，假设狗当前位置的概率分布为左图，狗在接下来1s内可能跑0m，1m，2m对应的离散概率分布为[0.1，0.8，0.1]。预测狗接下来的位置为右图。可以用卷积的方式表达。卷积之后的效果是，概率更加发散，所以prediction以后往往不确定性会提高。
  $$\overline{x_k}=\widehat{x_k}\ast f_x(\cdot )\text{\hspace{1em}(3.1)}$$
  
• update
  为了体现离散概率的特点，在tracking dog这个例子中，使用的位置传感器，不能直接给出位置，但是能判断狗当前是在门前还是墙前。传感器正确的概率0.75。根据传感器返回的结果$z_k$更新狗的位置的概率分布。
  $$P(\hat{x}|z)=\frac{P(z|\overline{x})P(\overline{x})}{P(z)}\text{\hspace{1em}(3.2)}$$
  这个公式代表这
  $$posterior=\frac{likelihood\ast prior}{normalization}\text{\hspace{1em}(3.3)}$$
  假设有10个离散的位置点，狗一开始随机在一个位置，位置的map用向量表示为[1,1,0,0,0,0,0,0,1,0] (1表示门，0表示墙)，每个位置点的概率是0.1*ones[10,1],然后传感器给出了狗在门前，更新之后狗所处位置的概率如下图。因而测量数据能降低不确定性。
  

4. 卡尔曼滤波

4.1 单变量卡尔曼滤波

4.1.1 卡尔曼滤波和离散贝叶斯滤波的联系

离散贝叶斯滤波有几个重要的缺陷

• 没办法计算多变量
• 计算太复杂，需要独立的计算每个离散点位置点的概率
• 可能会有多个结果，如果出现多个点的概率是一样的，就出现了多个结果
  在实际生活中，每个点的概率很可能遵循一定的概率分布，其中常见的概率分布是正态分布，所以单变量卡尔曼滤波将离散贝叶斯中的每个点的概率用正态分布表示。

4.1.2 predict 和 update

• predict
  $\overline{x}=\hat{x}+f(x)$,其中$f(x)$均值和方差分别为$\mu$和${\sigma }^2$,因为两个正态分布的期望和方差之和的满足下面的关系。
  $$\begin{array}{rl}\overline{x}& =\hat{x}+{\mu }_{f(x)}\\ {\overline{{\sigma }_x}}^2& ={\hat{\sigma }}^2+{\sigma }_{f(x)}^2\end{array}\text{\hspace{1em}(4.1)}$$
• update
  计算update，按照贝叶斯公式(3.2),只能按照正态分布的公式去计算所需要的结果。
  $$\begin{array}{rl}P(z_k|\widehat{x_k})& =\frac{1}{\sqrt{2\pi {\sigma }_z^2}}\exp (-\frac{(x-u_z{)}^2}{2{\sigma }_z^2})\\ P(\widehat{x_k})& =\frac{1}{\sqrt{2\pi {\sigma }_x^2}}\exp (-\frac{(x-u_x{)}^2}{2{\sigma }_x^2})\end{array}\text{\hspace{1em}(4.2)}$$
  因为$P(z_k|\widehat{x_k})P(\widehat{x_k})$的结果经过normalization之后仍然是正态分布。
  
  $$\begin{array}{rl}P(\widehat{x_k}|z_k)& \propto P(z_k|\overline{x_k})P(\overline{x_k})\\ & \propto \frac{1}{\sqrt{2\pi {\sigma }_x^2}}\exp (-\frac{(x-u_x{)}^2}{2{\sigma }_x^2})\\ & \propto \exp (-\frac{x^2({\sigma }_z^2+{\sigma }_x^2)-2x({\mu }_z{\sigma }_x^2+{\mu }_x{\sigma }_z^2)}{2{\sigma }_z^2{\sigma }_x^2})\\ & \propto \exp (-\frac{x-\frac{{\mu }_x{\sigma }_z^2+{\mu }_z{\sigma }_x^2}{{\sigma }_z^2+{\sigma }_x^2}}{\frac{{\sigma }_x^2{\sigma }_z^2}{{\sigma }_x^2+{\sigma }_y^2}})\end{array}\text{\hspace{1em}(4.3)}$$
  根据公式（4.3）可以推导出结合测量结果更新后的数据的期望和方差为
  $$\begin{array}{rl}\hat{x}& =\frac{{\mu }_z{\sigma }_{\overline{x}}^2+{\mu }_{\overline{x}}{\sigma }_z^2}{{\sigma }_{\overline{x}}^2+{\sigma }_z^2}\\ {\sigma }_{\hat{x}}^2& =\frac{{\sigma }_{\overline{x}}^2{\sigma }_z^2}{{\sigma }_{\overline{x}}^2+{\sigma }_z^2}\end{array}\text{\hspace{1em}(4.4)}$$

4.1.3 单变量卡尔曼滤波和gh滤波的联系

在gh滤波中，我们知道gh表示对测量数据的信任程度（范围是[0,1]）。
在单变量卡尔曼滤波中，令
$$g=\frac{{\sigma }_{\overline{x}}^2}{{\sigma }_{\overline{x}}^2+{\sigma }_z^2}\text{\hspace{1em}(4.5)}$$
update过程可以改写成：
$$\begin{array}{rl}\hat{x}& ={\mu }_{\overline{x}}+g({\mu }_z-{\mu }_{\overline{x}})\\ {\sigma }_{\hat{x}}^2& =g{\sigma }_z^2=(1-g){\sigma }_{\overline{x}}^2\end{array}\text{\hspace{1em}(4.6)}$$
$我们能观察到这里只有参数g，并没有出现了h？$
在单变量卡尔曼滤波中，v只能来源于自身的命令输入u，或者是其他的测速传感器。

4.1.4 单变量卡尔曼滤波和互补滤波器

互补滤波器的结构如下

• predict
  $$\overline{x_k}=\widehat{x_k}+f(x)\Delta t\text{\hspace{1em}(4.7)}$$
• update
  $$\begin{array}{rl}{\hat{x}}_{k+1}& =0.98\ast \overline{x_k}+0.02\ast z_k\\ & =\overline{x_k}+0.02(z_k-\overline{x_k})\end{array}\text{\hspace{1em}(4.8)}$$
  可以发现互补滤波器本质上就是一个最简单的gh 滤波。
• 低通滤波特性
  因为在互补滤波器中存在延时环节$\frac{z}{z-0.98}$,可以推广到所有的gh架构的滤波器都具有一定的低通滤波的功能，从这个角度来看，卡尔曼滤波本身也有一定的滤波的作用（经过measurement之后方差减小了，效果类似低通滤波）
  $$roll=(0.02\ast z_k+0.98\ast f(x)\Delta t)\frac{z}{z-0.98}\text{\hspace{1em}(4.9)}$$

图片来源于[2]

4.1.5 单变量卡尔曼滤波的缺陷

最重要的实际上我们知道$（x,\dot{x}）$是有很大的相关性，但是在单变量卡尔曼滤波中，变量$\dot{x}$的特性并没有利用上，和x之间的联系也没有用上。

4.2 多变量卡尔曼滤波

4.2.1 多变量正态分布

因为多变量之间存在一定的联系，所以多变量的采用协方差的方式进行定义。
例如身高和体重（W,H）是一对正相关的变量
$$\Sigma =\left[\begin{array}{cc}{\sigma }_H^2& {\sigma }_{HW}\\ {\sigma }_{WH}& {\sigma }_W^2\end{array}\right]\text{\hspace{1em}(4.10)}$$
正态分布的数学表达式为：
$$f(x,u,\Sigma )=\frac{1}{\sqrt{(2\pi {)}^n\Sigma }}\exp (-\frac{1}{2}(x-u{)}^T\Sigma (x-u))\text{\hspace{1em}(4.11)}$$
同样的需要计算两个多变量高斯分布的product和相加，在我们展开计算之前，尝试从几何的角度理解，product 和相加的影响，这里以二维正态分布为例。
对于标准的高斯分布，有65%的数据落在$[u-\sigma ,u+\sigma ]$, 有95%的数据落在$[u-2\sigma ,u+2\sigma ]$，有99.7%的数据落在$[u-3\sigma ,u+3\sigma ]$。以$3\sigma$为范围表示正态分布的数据，如下图。

• 高斯分布的product
  下面第一个图时协方差为0的二维高斯分布和协方差不为0的二维高斯分布的product，其结果的方差更小。下面第二个图是两个协方差不为0的高斯分布的product结果非常理想。
  
  
  对于两个两个相关的变量，在测量过程中，虽然仅仅位置可以被直接观察到，速度是hidden variable,但是只要他们之间存在着线性关系，位置和速度都会变得非常精确。
  
  $接下来需要非常关注，如何只观察位置，得到更加精确的位置和速度？$
  关于具体的高斯分布相加和product的公式，采用类比的方式进行理解
• 高斯分布相加
  
• 高斯分布product
  

4.2.2 线性动力学模型

考虑的模型噪声的线性动力学模型，可以表示为：
$$\dot{X}=AX+BU+w\text{\hspace{1em}(4.12)}$$
在时不变系统中，可以表示为
$$X(t_{k+1})=F(\Delta t)X{t}_k+B(t_k)U(t_k)+w\text{\hspace{1em}(4.13)}$$
问题是如何将A转换成$F(\Delta t)$，在无外部干扰的情况进行分析，根据欧拉法
$$X=\exp (At)X_0\text{\hspace{1em}(4.14)}$$
$\exp (At)$通过泰勒展开可以得到：
$$\exp (At)=I+At+\frac{(At{)}^2}{2!}+\frac{(At{)}^3}{3!}+o(At{)}^3\text{\hspace{1em}(4.15)}$$
我们在使用的时候根据模型的阶数，选择$\exp (At)$需要精确的位数

• 一阶系统 $x(t_{k+1})=\dot{x}(t_k)\Delta t+x(t_k)$
  改写成标准形式为：
  $$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]\text{\hspace{1em}(4.16)}$$
  则有
  $$A=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\text{\hspace{1em}(4.17)}$$
  这里精确到$\exp (At)$的一阶倒数
  $$F(t_k)=I+A\Delta t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\text{\hspace{1em}(4.18)}$$
• 二阶系统$x(t_{k+1})=0.5\ddot{x}(\Delta t{)}^2+\dot{x}(t_k)\Delta t+x(t_k)$
  简化成标准形式：
  $$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ {\ddot{x}}^{\prime }\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \ddot{x}\end{array}\right]\text{\hspace{1em}(4.19)}$$
  则有：
  $$A=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]\text{\hspace{1em}(4.20)}$$
  这里精确到$\exp (At)$的二阶倒数
  $$F(t_k)=I+A\Delta t+\frac{(A\Delta t{)}^2}{2!}=\left[\begin{array}{ccc}1& \Delta t& 0.5(\Delta t{)}^2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(4.21)}$$

4.2.3 多变量卡尔曼滤波

• predict
  期望是按照线性动力学公式推导过来的，没有什么好说的。
  方差可以从1维的角度进行类比
  $$\begin{array}{rl}\Sigma [\overline{x}{]}^2& =E[(Fx-F\mu {)}^2]\\ & =E[F(x-u{)}^2{F}^T]\\ & =E[F\Sigma {\Sigma }^T{F}^T]\\ & =FP{F}^T\end{array}\text{\hspace{1em}(4.22)}$$
  对于Q，有两种主要的处理方式。
  1）piecewise noise model: 以一阶系统为例,那么输入噪声w只对最高阶次（加速度）有影响，系统输入的噪声w对状态的影响可以表示为以下，
  $$\left[\begin{array}{c}x\\ \dot{x}\\ \ddot{x}\end{array}\right]=\left[\begin{array}{ccc}1& \Delta t& 0.5(\Delta t{)}^2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]w=\left[\begin{array}{c}0.5(\Delta t{)}^2\\ \Delta t\\ 1\end{array}\right]w=\Gamma w\text{\hspace{1em}(4.23)}$$
  那么w对状态变量$[x,\dot{x}]$的影响可以表示为：
  $$\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]=\Gamma w=\left[\begin{array}{c}0.5(\Delta t{)}^2\\ \Delta t\end{array}\right]w$$
  可以计算Q为
  $$Q=E[\Gamma w{w}^T{\Gamma }^T]=\Gamma {\sigma }_v^2{\Gamma }^T\text{\hspace{1em}(4..24)}$$
  在计算二阶系统的时候，也可以采用同样的方式，认为w作为在最高阶变量（jerk）上，这样太复杂，没有必要，一般用：
  $$\Gamma =\left[\begin{array}{c}0.5(\Delta t{)}^2\\ \Delta t\\ 1\end{array}\right]$$
  2)simplific form 上面这种方法比较发杂，以二阶系统为例，在工程里，有时候也直接将Q简化的表示成：
  $$Q=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& {\sigma }_v^2\end{array}\right]$$
  ${\sigma }_v的选择为[0.5\Delta a,\Delta a],其中\Delta a是单个采样周期内加速度最大变化值$。
  
• update
  以一阶系统为例，推导下面的关系。
  第一种情况，假设传感器只能测量位置信息
  $$\begin{array}{rl}{Z}_k& =HX+V_k\\ & =\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]+\left[\begin{array}{c}{v}_x\\ 0\end{array}\right]\end{array}\text{\hspace{1em}(4.25)}$$
  先把K放在一边（K计算比较麻烦），计算尝试取计算P。
  $$\begin{array}{rl}x-\hat{x}& =x-(\overline{x}+K(z-H\overline{x}))\\ & =x-(\overline{x}+K(Hx+V_k-H\overline{x}))\\ & =(I-KH)(x-\overline{x})-K{V}_k\end{array}\text{\hspace{1em}(4.26)}$$
  计算添加了测量数值后的方差,假设噪声是白噪声，即$E[V_k]=0$
  $$\begin{array}{rl}P& =E[(x-\hat{x})(x-\hat{x})]\\ & =E[((I-KH)(x-\overline{x})-K{V}_k)((I-KH)(x-\overline{x})-K{V}_k{)}^T]\\ & =(I-KH)(x-\overline{x})(x-\overline{x}{)}^T(I-KH{)}^T+K{V}_k{V}_k^T{K}^T-2(I-KH)(x-\overline{x})E[V_k{]}^T{K}^T\\ & =(I-KH)\overline{P}(I-KH{)}^T+KR{K}^T\end{array}\text{\hspace{1em}(4.27)}$$
  这样就得到了P，接下来就是研究K是如何计算的。希望取一个非常合理的K，使得P的数值最小。
  $$\begin{array}{r}\frac{dP}{dK}=0\end{array}\text{\hspace{1em}(4.28)}$$
  在因为P和K是矩阵，所以采用矩阵的trace计算倒数,在进一步计算之前，有两个公式至关重要(AB是方阵，C是对称矩阵)
  $$\begin{array}{rl}\frac{dtrace(AB)}{dA}& =B^T\\ \frac{dtrace(AC{A}^T)}{dA}& =2AC\end{array}\text{\hspace{1em}(4.29)}$$
  再结合（4.27和4.28）计算K
  $$\begin{array}{r}\frac{dtrace(P)}{dK}=2(KH-I)\overline{P}{H}^T+2KR=0\end{array}\text{\hspace{1em}(4.30)}$$
  根据（4.30）可以计算K为
  $$K=\overline{P}{H}^T(H\overline{P}{H}^T+R{)}^{-1}$$
  关于观测方差R，是一个m*m矩阵(m是传感器的个数)，例如有位置和速度两个传感器。
  $$R=\left[\begin{array}{cc}{\sigma }_x^2& 0\\ 0& {\sigma }_v^2\end{array}\right]$$
  传感器的方差就是传感器噪声的功率谱。
  
  理论部分的回顾到此结束，下一章节开始进入线性卡尔曼滤波的应用。

References

[1] https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
[2] https://www.youtube.com/watch?v=whSw42XddsU