Kalman滤波

Kalman滤波的感性认识

更简单明了的感性认识

对于线性系统一般状态方程涉及到两个，由这两个方程估计状态量：

1. 运动方程： $x_k=f(x_{k-1},u_k)+{\omega }_k$ （由上一时刻状态怎么得到当前时刻的状态）
2. 观测方程：$z_k=h(x_k)+v_k$（在当前时刻的状态下产生了怎样的观测）

假设所有分布都是高斯分布，从上时刻$x_{k-1}$根据运动方程得到的$x_k$ 服从一个高斯分布$G_1$；我们得到的观测（传感器直接测量） $z_k$也是高斯分布，从它根据观测方程推出的$x_k$是另一个高斯分布$G_2$；最后我们由两个状态方程得到关于$x_k$的两个高斯分布$G_1和G_2$，卡尔曼滤波结果就是把$G_1和G_2$相乘得到更确定的估计。

相关高斯分布公式总结

• 一维高斯分布：$$N(\mu ,{\sigma }^2)=\frac{1}{\sqrt{2\pi }\sigma }exp\left(-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}\right)$$

• 多维高斯分布：$$N(\mathbf{\mu },\Sigma )=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp\left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\top }{\Sigma }^{-1}(\mathbf{x}-\mathbf{\mu })\right)$$

• 两个独立的高斯分布：$\mathbf{x}\sim N({\mathbf{\mu }}_{\mathbf{x}},{\mathbf{\Sigma }}_{\mathbf{x}\mathbf{x}})$ 和$\mathbf{y}\sim N({\mathbf{\mu }}_{\mathbf{y}},{\mathbf{\Sigma }}_{\mathbf{y}\mathbf{y}})$相加的结果依然是高斯分布：$$\mathbf{x}+\mathbf{y}\sim N({\mathbf{\mu }}_{\mathbf{x}}+{\mathbf{\mu }}_{\mathbf{y}},{\mathbf{\Sigma }}_{\mathbf{x}\mathbf{x}}+{\mathbf{\Sigma }}_{\mathbf{y}\mathbf{y}})$$
  
  
• 如果一个常数$a$乘以高斯分布$\mathbf{x}\sim N({\mathbf{\mu }}_{\mathbf{x}},{\mathbf{\Sigma }}_{\mathbf{x}\mathbf{x}})$，那么$a\mathbf{x}$服从：$$a\mathbf{x}\sim N(a{\mathbf{\mu }}_{\mathbf{x}},a^2{\mathbf{\Sigma }}_{\mathbf{x}\mathbf{x}})$$
  
  
• 如果 $\mathbf{y}=\mathbf{A}\mathbf{x}$，$\mathbf{x}\sim N({\mathbf{\mu }}_{\mathbf{x}},{\mathbf{\Sigma }}_{\mathbf{x}\mathbf{x}})$，则$\mathbf{y}$满足：$$\mathbf{y}\sim N(\mathbf{A}{\mathbf{\mu }}_{\mathbf{x}},\mathbf{A}{\mathbf{\Sigma }}_{\mathbf{x}\mathbf{x}}{\mathbf{A}}^{\mathbf{T}})$$
  
  
• 两个高斯分布相乘，如果是一维： $x\sim N({\mu }_x,{\sigma }_x^2)y\sim N({\mu }_y,{\sigma }_y^2)$，那么$x\cdot y\sim N(\mu ,{\sigma }^2)$：
  $$\begin{gathered} \mu ={\mu }_x+\frac{{\sigma }_x^2({\mu }_y-{\mu }_x)}{{\sigma }_x^2+{\sigma }_y^2} \\ {\sigma }^2={\sigma }_x^2-\frac{{\sigma }_x^4}{{\sigma }_x^2+{\sigma }_y^2} \end{gathered}$$
  如果令 $k=\frac{{\sigma }_x^2}{{\sigma }_x^2+{\sigma }_y^2}$，那么：
  $$\begin{gathered} \mu ={\mu }_x+k({\mu }_y-{\mu }_x) \\ {\sigma }^2={\sigma }_x^2-k{\sigma }_x^2 \end{gathered}$$
  扩展到多维 $\mathbf{x}\sim N({\mathbf{\mu }}_{\mathbf{x}},{\Sigma }_{xx})\mathbf{y}\sim N({\mathbf{\mu }}_{\mathbf{y}},{\Sigma }_{yy})$，那么$\mathbf{x}\cdot \mathbf{y}\sim N(\mathbf{\mu },\Sigma )$：$$\begin{gathered} K={\Sigma }_{xx}({\Sigma }_{xx}+{\Sigma }_{yy}{)}^{-1} \\ \mathbf{\mu }={\mathbf{\mu }}_{\mathbf{x}}+K({\mathbf{\mu }}_{\mathbf{y}}-{\mathbf{\mu }}_{\mathbf{x}}) \\ \Sigma ={\Sigma }_{xx}-K{\Sigma }_{xx} \end{gathered}$$

线性卡尔曼滤波

好理解Kalman参考

理论推导

假设线性系统的状态方程：$$\{\begin{array}{rl}{\mathbf{x}}_{\mathbf{k}}& =A{\mathbf{x}}_{\mathbf{k}-1}+B{\mathbf{u}}_{\mathbf{k}}+{\mathbf{w}}_{\mathbf{k}-1}\\ {\mathbf{z}}_{\mathbf{k}}& =H{\mathbf{x}}_{\mathbf{k}}+{\mathbf{v}}_{\mathbf{k}}\end{array}$$
$\mathbf{x}$：状态向量
$A$：状态转移矩阵
$B$：控制输入矩阵
$\mathbf{u}$：输入量
$\mathbf{w}$：过程噪声，服从均值为0，协方差为R的高斯分布 $N(0,R)$
$\mathbf{z}$：观测值
$H$：状态观测矩阵
$\mathbf{v}$：测量噪声，服从均值为0，协方差为Q的高斯分布 $N(0,Q)$

规定：
${\mathbf{x}}_{\mathbf{k}}$: 真实值
$\widehat{{\mathbf{x}}_{\mathbf{k}}}$: 状态向量最优估计均值（也是最后的要求的值）
${\hat{P}}_k$: 最优估计状态向量的协方差
$\overline{{\mathbf{x}}_{\mathbf{k}}}$: 预测的状态向量均值
${\bar{P}}_k$: 预测的状态向量协方差

推导过程从贝叶斯公式的角度，求的最大后验概率：$$p(x_k|z_k)\propto p(z_k|x_k)p(x_k)$$

1. 预测（从上一时刻${\mathbf{x}}_{\mathbf{k}-1}$根据运动方程得到${\mathbf{x}}_{\mathbf{k}}$的先验分布）
   因为${\mathbf{x}}_{\mathbf{k}-1}$是上一时刻的最优估计，服从$N(\widehat{{\mathbf{x}}_{\mathbf{k}-1}},{\hat{P}}_{k-1})$, 根据运动方程得到k时刻预测(先验)：
   $$p({\mathbf{x}}_{\mathbf{k}})=N(A\widehat{{\mathbf{x}}_{\mathbf{k}-1}}+B{\mathbf{u}}_k,A{\hat{P}}_{k-1}{A}^T+R)$$即：
   $$\{\begin{array}{rl}\overline{{\mathbf{x}}_{\mathbf{k}}}& =A\widehat{{\mathbf{x}}_{\mathbf{k}-1}}+B{\mathbf{u}}_k\\ {\bar{P}}_k& =A{\hat{P}}_{k-1}{A}^T+R\end{array}$$

2. 在先验下产生了怎样的观测（似然部分）
   由观测方程得到：$$p({\mathbf{z}}_{\mathbf{k}}|{\mathbf{x}}_{\mathbf{k}})=N(H{\mathbf{x}}_{\mathbf{k}},Q)$$
   ${\mathbf{x}}_{\mathbf{k}}$（上一步预测的，不确定性已经体现在先验里面，所以这里是一个协方差为0的确定值）

3. 由似然和先验求后验
   $$N(\widehat{x_k},\widehat{P_k})=N(H{\mathbf{x}}_{\mathbf{k}},Q)\cdot N(\overline{x_k},\overline{P_k})$$

根据多维高斯公式把左右两边展开，然后比较指数部分：
$$({\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}}{)}^T{\hat{P}}_k^{-1}({\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}})=({\mathbf{z}}_{\mathbf{k}}-H{\mathbf{x}}_{\mathbf{k}}{)}^T{Q}^{-1}({\mathbf{z}}_{\mathbf{k}}-H{\mathbf{x}}_{\mathbf{k}})+({\mathbf{x}}_{\mathbf{k}}-{\bar{\mathbf{x}}}_{\mathbf{k}}{)}^T{\bar{P}}_k^{-1}({\mathbf{x}}_{\mathbf{k}}-{\bar{\mathbf{x}}}_{\mathbf{k}})$$
把公式展开比较${\mathbf{x}}_{\mathbf{k}}$的二次和一次项系数：

• 对于二次项：$${\hat{P}}_k^{-1}=H^T{Q}^{-1}H+{\bar{P}}_k^{-1}$$
  左右同乘${\hat{P}}_k$得：$$I={\hat{P}}_k{H}^T{Q}^{-1}H+{\hat{P}}_k{\bar{P}}_k^{-1}$$
  定义中间变量$K={\hat{P}}_k{H}^T{Q}^{-1}$可得(这里定义的K只是为了方便推导，实际操作不这么算K，因为里面有后验，这是我们需要求的)：
  $$I=KH+{\hat{P}}_k{\bar{P}}_k^{-1}$$
  $${\hat{P}}_k=(I-KH){\bar{P}}_k$$

• 对于一次项：$$-2{{\hat{\mathbf{x}}}_{\mathbf{k}}}^T{\hat{P}}_k^{-1}=-2{{\mathbf{z}}_{\mathbf{k}}}^T{Q}^{-1}H-2{{\bar{\mathbf{x}}}_{\mathbf{k}}}^T{\bar{P}}_k^{-1}$$
  整理得：$${\hat{P}}_k^{-1}{\hat{\mathbf{x}}}_{\mathbf{k}}=H^T{Q}^{-1}{\mathbf{z}}_{\mathbf{k}}+{\bar{P}}_k^{-1}\overline{{\mathbf{x}}_k}$$
  两侧同时乘${\hat{P}}_k$得：$$\{\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\hat{P}}_k{H}^T{Q}^{-1}{\mathbf{z}}_{\mathbf{k}}+{\hat{P}}_k{\bar{P}}_k^{-1}{\bar{\mathbf{x}}}_k\\ & =K{\mathbf{z}}_{\mathbf{k}}+(I-KH){\bar{\mathbf{x}}}_k\end{array}$$
  则：$${\hat{\mathbf{x}}}_k={\bar{\mathbf{x}}}_k+K({\mathbf{z}}_{\mathbf{k}}-H{\bar{\mathbf{x}}}_k)$$

Kalman的过程

对于线性系统
$$\{\begin{array}{rl}{\mathbf{x}}_{\mathbf{k}}& =A{\mathbf{x}}_{\mathbf{k}-1}+B{\mathbf{u}}_{\mathbf{k}}+{\mathbf{w}}_{\mathbf{k}-1}\\ {\mathbf{z}}_{\mathbf{k}}& =H{\mathbf{x}}_{\mathbf{k}}+{\mathbf{v}}_{\mathbf{k}}\end{array}$$

1. 预测：
   $$\{\begin{array}{rl}\overline{{\mathbf{x}}_{\mathbf{k}}}& =A\widehat{{\mathbf{x}}_{\mathbf{k}-1}}+B{\mathbf{u}}_k\\ {\bar{P}}_k& =A{\hat{P}}_{k-1}{A}^T+R\end{array}$$
2. 更新
   $$\{\begin{array}{rl}K& ={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+Q{)}^{-1}\\ {\hat{\mathbf{x}}}_k& ={\bar{\mathbf{x}}}_k+K({\mathbf{z}}_{\mathbf{k}}-H{\bar{\mathbf{x}}}_k)\\ {\hat{P}}_k& =(I-KH){\bar{P}}_k\end{array}$$

应用代码

扩展卡尔曼滤波（EKF）

对于线性系统，KF是对系统的最优估计，但是实际应用中往往都是非线性系统，这时候我们需要利用近似，将系统方程在每个时刻近似为线性系统。

理论推导

应用代码

无迹卡尔曼滤波（UKF）

理论推导

应用代码