卡尔曼滤波（Kalman Filter）

一、卡尔曼滤波的背景与直观理解

1. 什么是卡尔曼滤波？

卡尔曼滤波（Kalman Filter）是一种用于从含噪声的数据中估计真实状态的算法。想象你在追踪一个移动物体（如一辆车），你有两个信息来源：

• 预测：根据物理规律（如速度和时间），推测它下一秒的位置。
• 测量：通过传感器（如GPS）直接测量其位置。

但存在问题：

• 预测可能不准确，因为无法确定车辆是否会突然加速或刹车（存在“过程噪声”）。
• 测量也不可靠，因为GPS信号可能受天气或建筑物干扰（存在“测量噪声”）。

卡尔曼滤波如同一位“智能中介”，结合预测和测量，找到一个既考虑物理规律又参考测量数据的“最佳估计”。它通过数学方法，自动决定何时更信任预测，何时更信任测量。

2. 为什么需要卡尔曼滤波？

现实世界的数据总是“脏”的：

• 传感器会出错（例如GPS位置漂移）。
• 系统数学模型不完美（例如未知的风速或路面摩擦力）。
  卡尔曼滤波的强大之处在于，它能实时、递归地处理这些不完美的数据，提供更接近真实值的估计。

3. 核心假设

卡尔曼滤波基于以下关键假设：

• 系统是线性的（状态变化和测量可用线性方程描述）。
• 噪声是高斯噪声（噪声呈钟形曲线，随机但有规律）。
• 初始状态和噪声相互独立。

若系统不满足这些假设（如非线性），可使用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。

4. 应用场景

• 机器人：帮助机器人确定自身位置（定位），如扫地机器人避开家具。
• 自动驾驶：跟踪周围车辆的位置和速度。
• 金融：预测股价趋势，过滤噪声。
• 航天：卫星导航、火箭轨迹控制。

---

二、卡尔曼滤波的数学模型

卡尔曼滤波的核心是两个方程：状态转移方程和测量方程，它们描述了系统的动态和传感器的工作方式。

1. 状态转移方程（系统如何变化）

状态转移方程为：$x_k=A{x}_{k-1}+B{u}_k+w_k$

• $x_k$：时刻 $k$ 的状态向量。例如，状态可以是车辆的$[p_k,v_k{]}^T$，表示位置和速度。
• $x_{k-1}$：上一时刻的状态。
• $A$：状态转移矩阵，描述状态随时间的变化。例如，若位置 $p_k=p_{k-1}+v_{k-1}\cdot \Delta t$，速度不变 $v_k=v_{k-1}$，则：$A=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$。
• $u_k$：控制输入（可选），如油门或刹车。若无控制，$u_k=0$，$B$ 可忽略。
• $w_k$：过程噪声，表示系统的不确定性（如路面不平）。假设其为高斯噪声，均值为0，协方差为 $Q$（$w_k\sim N(0,Q)$）。

直观理解：此方程表示：“根据上一秒的车辆位置和速度，预测下一秒的位置，但可能存在误差（噪声）。”

2. 测量方程（传感器如何观测）

测量方程为：$z_k=H{x}_k+v_k$

• $z_k$：时刻 $k$ 的测量值。例如，GPS提供的车辆位置。
• $H$：测量矩阵，将状态映射到测量。例如，若仅测量位置，忽略速度，则：$H=\left[\begin{array}{cc}1& 0\end{array}\right]$，因为 $z_k=p_k$（仅测位置）。
• $v_k$：测量噪声，表示传感器误差（如GPS不准）。假设其为高斯噪声，均值为0，协方差为 $R$（$v_k\sim N(0,R)$）。

直观理解：此方程表示：“传感器测得一个值，但可能偏离真实状态，因存在噪声。”

3. 协方差矩阵 $Q$ 和 $R$

• $Q$：过程噪声协方差，描述系统模型的不确定性（如风速变化）。$Q$ 越大，预测越不可靠。
• $R$：测量噪声协方差，描述传感器的不确定性（如GPS误差）。$R$ 越大，测量越不可靠。

类比：$Q$ 和 $R$ 分别表示“预测有多不可靠”和“测量有多不可靠”。卡尔曼滤波根据它们决定“更信任谁”。

---

三、卡尔曼滤波的算法步骤

卡尔曼滤波是一个预测-更新的循环，每次循环分为两步：

1. 预测（时间更新）：根据系统模型，预测当前状态。
2. 更新（测量更新）：利用传感器数据修正预测，得到更准确的估计。

1. 符号说明

• ${\hat{x}}_{k|k-1}$：先验状态估计，时刻 $k$ 的预测值（未用测量修正）。
• ${\hat{x}}_{k|k}$：后验状态估计，时刻 $k$ 的最终估计（经测量修正）。
• $P_{k|k-1}$：先验协方差，预测的不确定性。
• $P_{k|k}$：后验协方差，修正后的不确定性。
• $K_k$：卡尔曼增益，决定预测和测量的权重。

2. 预测步骤（时间更新）

（1）状态预测：
${\hat{x}}_{k|k-1}=A{\hat{x}}_{k-1|k-1}+B{u}_k$

• 使用上一时刻的最终估计 ${\hat{x}}_{k-1|k-1}$，通过状态转移矩阵 $A$ 预测当前状态。
• 若有控制输入（如油门），加上 $B{u}_k$。
• 直观理解：根据“昨天的车辆位置和速度”，预测“今天的位置”。

（2）协方差预测：
$P_{k|k-1}=A{P}_{k-1|k-1}{A}^T+Q$

• $P_{k-1|k-1}$ 是上一时刻的不确定性，经 $A$ 变换后，成为当前的不确定性。
• 加上过程噪声 $Q$，因预测存在误差。
• 直观理解：预测越远，不确定性越大（如预测明天天气比今天更不准）。

3. 更新步骤（测量更新）

（1）卡尔曼增益：
$K_k=P_{k|k-1}{H}^T(H{P}_{k|k-1}{H}^T+R{)}^{-1}$

• $K_k$ 是一个权重，决定预测和测量的相对重要性。
• 若 $R$（测量噪声）大，$K_k$ 小，说明测量不可靠，更信任预测。
• 若 $P_{k|k-1}$（预测不确定性）大，$K_k$ 大，说明预测不可靠，更信任测量。
• 直观理解：$K_k$ 像“裁判”，根据“预测和测量的可信度”分配权重。

（2）状态更新：
x^k∣k=x^k∣k−1+Kk(zk−Hx^k∣k−1)\hat{x}_{k|k} = \hat{x}_{k|k-1}