卡尔曼滤波 (Kalman Filter)

这一周的视频质量很一般，slides 做得也不仔细有不少错误。最后看了一些其他网站的文章和视频才有了比较深刻的理解，本文结尾有推荐资料。

KF 模型

  • 以匀速运动的动力学模型为例
    • 状态 \(x_{t+1}=A x_t + B u_t\)，\(u_t\) 为外部影响
    • 观测结果 \(z_t = C x_t\)
    • \(x_t\) 和 \(z_t\) 都有测量误差
    • 状态 \(x_{t+1}\) 仅包含位置和速度：\(x_{t+1} := \begin{bmatrix}v & \frac{dv}{dt}\end{bmatrix}\)
    • 动力学模型可以通过矩阵 A 描述，\(A = \begin{bmatrix}1 & dt \\ 0 & 1 \end{bmatrix}\)

基于动力学的贝叶斯模型 \[ p(x_{t+1}|x_t) = Ap(x_t) \\ p(z_t|x_t) = Cp(x_t) \]

加入运动和观测的误差 \[ p(x_{t+1}|x_t) = Ap(x_t)+v_m \\ p(z_t|x_t) = Cp(x_t)+v_0 \]

假设误差基于高斯分布 \[ p(x_{t+1}|x_t) = A\mathcal{N}(x_t, P_t) + \mathcal{N}(0, \Sigma_m) \\ p(z_t|x_t) = C\mathcal{N}(x_t, P_t) + \mathcal{N}(0, \Sigma_0) \]

把线性变换 A 和 C 代入正态分布 \[ p(x_{t+1}|x_t) = \mathcal{N}(Ax_t, A P_t A^T) + \mathcal{N}(0, \Sigma_m) \\ p(z_t|x_t) = \mathcal{N}(Cx_t, C P_t C^T) + \mathcal{N}(0, \Sigma_0) \]

线性加和 \[ p(x_{t+1}|x_t) = \mathcal{N}(Ax_t, A P_t A^T+ \Sigma_m) \\ p(z_t|x_t) = \mathcal{N}(Cx_t, C P_t C^T + \Sigma_0) \]

最大后验概率估计卡尔曼滤波

最大后验概率估计的目的在于最大化 \(p(x_t | z_t, x_{t-1})\)，即 \(\hat{x_t} = \argmax_{x_t} p(x_t | z_t, x_{t-1})\)

由贝叶斯公式 \[ p(x_t | z_t, x_{t-1}) = \frac{p(z_t | x_t, x_{t-1}) p(x_t | x_{t-1})}{P (z_t)} \] 代入正态分布得 \[ \hat{x_t} = \argmax_{x_t} \mathcal{N}(Cx_t, C P_t C^T + \Sigma_0) \mathcal{N}(Ax_t, A P_t A^T+ \Sigma_m) \] 求导可解得（具体过程参考 slides） \[ \begin{align} \hat{x_t} &= A x_{t-1} + K(z_t - C A x_{t-1}) \\ \hat{P_t} &= P - KCP \\ K &= P C^T R^{-1} - K C P C^T R^{-1} \\ P &= A P_{t-1} A^T + \Sigma_m \\ R &= \Sigma_0 \end{align} \]

这里 K 就是 Kalman gain。

扩展卡尔曼滤波 (Extended Kalman Filter)

KF 的局限之一在于假设了线性模型，EKF 去掉了线性模型的限制，接受任何 \[ x_{t+1} = A x_t + B u_t => x_{t+1} = f(x_t, u_t) \\ z_t = C x_t => z_t = h(x_t) \]

无迹卡尔曼滤波 (Unscented Kalman Filter)

参考资料