卡尔曼滤波（Kalman Filter）公式【SLAM】

卡尔曼滤波

核心公式

运动方程：
$$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$$
$$w_k\sim \mathcal{N}(0,Q_k)$$
观测方程：
$$z_k=C_k{x}_k+v_k$$
$$v_k\sim \mathcal{N}(0,R_k)$$

预测：¹
先验状态量均值（Predicted (a priori) state estimate）：
$${\check{x}}_k=A_k{\hat{x}}_{k-1}+B_k{u}_k$$
先验状态量协方差（Predicted (a priori) estimate covariance）：
$${\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+Q_k$$
更新：
状态更新前的观测协方差，先验观测协方差（Innovation (or pre-fit residual) covariance）：
$$S_k=C_k{\check{P}}_k{C}_k^T+R_k$$
卡尔曼增益（Optimal Kalman gain）：
$$K_k=\frac{{\check{P}}_k{C}_k^T}{S_k}=\frac{{\check{P}}_k{C}_k^T}{C_k{\check{P}}_k{C}_k^T+R_k}$$
状态更新前，观测值减去预测值得到的残差；未经状态更新的测量残差（
Innovation or measurement pre-fit residual）：
$${\check{y}}_k=z_k-C_k{\check{x}}_k$$
后验状态量均值（Updated (a posteriori) state estimate）：
$${\hat{x}}_k={\check{x}}_k+K_k{\check{y}}_k$$
后验状态量协方差（Updated (a posteriori) estimate covariance）：
$${\hat{P}}_k=(I-K_k{C}_k){\check{P}}_k$$
更新后测量残差（Measurement post-fit residual）：
$${\hat{y}}_k=z_k-C_k{\hat{x}}_k$$

---

1. 本人翻译可能不准确，所以给出概念的英文描述，资料来自Wikipedia:Kalman filter ↩︎