算法 - 卡尔曼滤波(KF)

基本概念

卡尔曼滤波的基本任务是根据系统动态模型和传感器的观测值，递归地估计系统的状态。换句话说，卡尔曼滤波结合了对系统行为的预测和传感器测量的更新，从而得到一个更精确的状态估计。

• 预测： 根据上一时刻的状态估计和控制输入，预测当前时刻的状态和误差协方差。
• 更新： 根据实际测量值，更新当前时刻的状态估计，调整预测结果。

工作流程

假定你有一个系统，例如温度控制系统，卡尔曼滤波的流程大致为：

1. 初始化： 设定初始的状态估计 $\hat{x}_0$ 和初始误差协方差矩阵 $P_0$ 。

2. 预测： 每当新的时间步到来时，根据系统模型（例如温度加热过程的方程）预测当前的系统状态 $\hat{x}^{-}_{k} 和误差协方差 $P^{-}_{k}$ 。

3. 更新： 通过获取新的测量值（例如，温度传感器的度数），计算卡尔曼增益 $K_k$ ，并根据新的测量值更新状态估计 $\hat{x}_k$ 和误差协方差 $P_k$ 。

五大公式

卡尔曼滤波算法总共有五个核心公式，分为预测、更新两大部分。

预测(Prediction)

基于当前的系统状态和控制输入，预测下一个时刻的状态，此部分有两个步骤如下：

1. 状态预测：

$$\hat{x}^{-}_{k} = A\hat{x}_{k-1} + Bu_k$$

• $\hat{x}^{-}_{k}$ 时对当前状态的预测值。

• $A$ 是状态转移矩阵（描述系统从上一个状态到当前状态的变化规律）。

• $\hat{x}_{k-1}$ 是上一时刻的估计状态。

• $B$ 是控制输入矩阵（描述控制输入如何影响系统状态）。

• $u_k$ 是控制输入。

2. 误差协方差预测：

$$P^{-}_{k} = AP_{k-1}A^T + Q$$

• $P^{-}_{k}$ 是当前时刻的误差协方差矩阵，描述预测误差的大小。

• $P_{k-1}$ 是上一时刻的误差协方差矩阵。

• $Q$ 是过程噪声协方差矩阵，描述系统模型本身的噪声。

更新(Update)

将预测值与实际测量值结合，并更新当前的状态估计，此部分有三个步骤如下：

1. 卡尔曼增益 (Kalman Gain)

$$K_k = P^{-}_{k} H^T (H P^{-}_{k} H^{T} + R)^{-1}$$

• $K_k$ 是卡尔曼增益，决定了预测值和测量值的权重。

• $H$ 是观测矩阵，将状态空间映射到测量空间。

• $R$ 是测量噪声协方差矩阵，描述观测噪声的大小。

2. 状态更新

$$\hat{x}_k = \hat{x}^{-}_{k} + K_k (z_k - H \hat{x}^{-}_{k})$$

• $\hat{x}_k$ 是更新后的估计状态，即后验估计值。

• $z_k$ 是当前时刻的实际测量值。

• $H \hat{x}^{-}_{k}$ 是根据预测值计算的测量值。

3. 误差协方差更新

$$P_k = (1 - K_k H) P^{-}_{k}$$

• $P_k$ 是更新后的误差协方差矩阵。

卡尔曼滤波的优点

• 最优估计： 卡尔曼滤波通过加权预测值和测量值，能够在噪声和误差的干扰下提供最优估计。
• 递归计算： 卡尔曼滤波是一种递归算法，不需要存储大量的历史数据，只需要当前状态的估计和误差协方差，适合实时计算。
• 适应性强： 卡尔曼滤波能够自适应地调整预测和更新过程，适应动态系统的变化。

卡尔曼滤波与高斯加权移动平均滤波的比较

高斯加权移动平均滤波是一个简单的平滑方法，它基于一定大小的滑动窗口和权重系数来平均数据。它适合于静态或者变化较慢的信号，但对态变化的信号，效果较差，且不能预测未来的状态。
卡尔曼滤波则是一个基于模型的滤波方法，它能够结合系统模型和测量数据，通过递归更新来提供更精确的状态估计。它适合于动态变化的系统，能够有效地处理噪声和滞后，特别是在系统本身有动态变化时，卡尔曼滤波的优势更加明显。

总结

卡尔曼滤波是一种非常强大的滤波方法，尤其适合于动态系统、具有噪声或滞后效应的系统。与简单的高斯加权移动平均滤波相比，卡尔曼滤波能够根据系统的动态模型和测量数据，自适应地调整估计，并在动态变化的环境中提供最优的状态估计。