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}$满足： y ∼ N ( A μ x , A Σ x x A T ) \bm{y}\sim N(\bm{A\mu_{x}}, \bm{A\Sigma_{xx}A^T}) y∼N(Aμx​,AΣ