Kalman滤波

文章目录

• Kalman滤波的感性认识
• 相关高斯分布公式总结
• 线性卡尔曼滤波
• 理论推导
• 应用代码
• 扩展卡尔曼滤波（EKF）
• 理论推导
• 应用代码
• 无迹卡尔曼滤波（UKF）
• 理论推导
• 应用代码

Kalman滤波的感性认识

更简单明了的感性认识

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

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

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

相关高斯分布公式总结

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

• 多维高斯分布：$N(\bm{ \mu }, \Sigma)=\frac{1}{\sqrt{(2\pi)^{N}det(\Sigma)}}exp\left ( -\frac{1}{2} (\bm{x}-\bm{\mu})^\top \Sigma^{-1} (\bm{x}-\bm{\mu})\right )$N(μ,Σ)=(2π)Ndet(Σ)​1​exp(−21​(x−μ)⊤Σ−1(x−μ))

• 两个独立的高斯分布：$\bm{x}\sim N(\bm{\mu_{x}}, \bm{\Sigma_{xx}})$x∼N(μx​,Σxx​) 和$\bm{y}\sim N(\bm{\mu_{y}}, \bm{\Sigma_{yy}})$y∼N(μy​,Σyy​)相加的结果依然是高斯分布：$\bm{x}+\bm{y} \sim N(\bm{\mu_{x}}+\bm{\mu_{y}},\bm{\Sigma_{xx}}+\bm{\Sigma_{yy}})$x+y∼N(μx​+μy​,Σxx​+Σyy​)
  
  
• 如果一个常数$a$a乘以高斯分布$\bm{x}\sim N(\bm{\mu_{x}}, \bm{\Sigma_{xx}})$x∼N(μx​,Σxx​)，那么$a\bm{x}$ax服从：$a\bm{x}\sim N(a\bm{\mu_{x}}, a^2\bm{\Sigma_{xx}})$ax∼N(aμx​,a2Σxx​)
  
  
• 如果 $\bm{y}=\bm{Ax}$y=Ax，$\bm{x}\sim N(\bm{\mu_{x}}, \bm{\Sigma_{xx}})$x∼N(μx​,Σxx​)，则$\bm{y}$y满足：$\bm{y}\sim N(\bm{A\mu_{x}}, \bm{A\Sigma_{xx}A^T})$y∼N(Aμx​,AΣxx​AT)
  
  
• 两个高斯分布相乘，如果是一维： $x\sim N(\mu_x, \sigma_x^2) \qquad y\sim N(\mu_y,\sigma_y^2)$x∼N(μx​,σx2​)y∼N(μy​,σy2​)，那么$x\cdot y\sim N(\mu, \sigma^2)$x⋅y∼N(μ,σ2)：
  $\mu = \mu_x +\frac{\sigma_x^2(\mu_y - \mu_x)}{\sigma_x^2+\sigma_y^2} \\ \quad \\ \sigma^2 = \sigma_x^2-\frac{\sigma_x^4}{\sigma_x^2+\sigma_y^2}$μ=μx​+σx2​+σy2​σx2​(μy​−μx​)​σ2=σx2​−σx2​+σy2​σx4​​
  如果令 $k=\frac{\sigma_x^2}{\sigma_x^2+\sigma_y^2}$k=σx2​+σy2​σx2​​，那么：
  $\mu = \mu_x + k(\mu_y-\mu_x) \\ \quad \\ \sigma^2= \sigma_x^2-k\sigma_x^2$μ=μx​+k(μy​−μx​)σ2=σx2​−kσx2​
  扩展到多维 $\bm{x}\sim N(\bm{\mu_x}, \Sigma_{xx}) \qquad \bm{y}\sim N(\bm{\mu_y},\Sigma_{yy})$x∼N(μx​,Σxx​)y∼N(μy​,Σyy​)，那么$\bm{x\cdot y} \sim N(\bm{\mu}, \Sigma)$x⋅y∼N(μ,Σ)：$K=\Sigma_{xx}(\Sigma_{xx}+\Sigma_{yy})^{-1} \\ \quad \\ \bm{\mu}= \bm{\mu_x} + K(\bm{\mu_y-\mu_x}) \\ \quad \\ \Sigma =\Sigma_{xx} -K \Sigma_{xx}$K=Σxx​(Σxx​+Σyy​)−1μ=μx​+K(μy​−μx​)Σ=Σxx​−KΣxx​

线性卡尔曼滤波

好理解Kalman参考

理论推导

1. 假设线性系统的状态方程：$\left\{\begin{aligned} \bm{x_k}&= A_k \bm{x_{k-1}}+\bm{u_k}+\bm{w_k} \\ \bm{z_k}&= H_k\bm{x_k}+\bm{v_k} \end{aligned}\right.${xk​zk​​=Ak​xk−1​+uk​+wk​=Hk​xk​+vk​​

$\bm{x_k}$xk​：状态向量
$\bm{u_k}$uk​：输入

https://zhuanlan.zhihu.com/p/48876718
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mathybits

应用代码

扩展卡尔曼滤波（EKF）

理论推导

应用代码

无迹卡尔曼滤波（UKF）

理论推导

应用代码