Kalmam Filter原理和公式推导

经典的卡尔曼黄金五条公式

参考链接

卡尔曼滤波器的主要作用

卡尔曼滤波器主要是利用了预测和更新两个过程来对系统状态进行最优估计。可以用在多传感器融合上，例如imu，odom，GPS信号的融合位姿估计。也就是利用对其他量的观测去估计无法直接测量的量。

卡尔曼滤波基本模型

$$x_k = A * x_{k-1} + B* u_k + w_{k-1} \\ z_k = H *x_k + v_k$$

$x_k$ 是系统状态矩阵，$z_k$ 是系统观测量，$A$ 是状态转移矩阵，$H$ 是状态观测矩阵，$w_{k-1}$ 是过程噪声，$v_k$ 是测量噪声。 其协方差法分别为$Q$ 和 $R$。

$x_k$ : 状态的真实值

$\tilde{x}_k^-$ : 状态的预测值，也称先验状态估计

$\tilde{x}_k$：状态的最优估计值 ，也称后验状态估计

黄金一条（状态预测方程）

$$\tilde{x}_k^- = A * \tilde{x}_{k-1} + B * u_k$$

黄金二条（状态更新方程）

$$\tilde{x}_k = \tilde{x}_k^- + K(z_k - H * \tilde{x}_k^-)$$

黄金三条（卡尔曼增益矩阵）

$$K = P_k^-H^T(HP_k^-H^T + R)^{-1}$$

黄金四条（估计误差方差阵）

$$P_k = (I - KH) * P_k^-$$

黄金五条（预测协方差阵）

$$P_{k+1}^- = AP_kA^T + Q$$

总结

卡尔曼滤波的主要思想就在于预测和更新，初始的协方差阵可以设为单位阵，之后不断地更新预测协方差阵和误差协方差阵，之后计算出K，然后根据K来更新最优估计。

ps：如果卡尔曼滤波没有预测方程会怎么样？

回答：如果没有 $\tilde{x}_k^- = A * \tilde{x}_{k-1} + B * u_k$ 的话，只有 $\tilde{x}_k^- = \tilde{x}_{k-1}$ 那么模型的估计误差方差阵会越来越小，导致K越来越接近0，最后机器人会认为自身不动。