卡尔曼滤波

基本概念

卡尔曼滤波是一种最优递归估计算法，用于从包含噪声的观测数据中估计动态系统的状态。它通过结合系统模型预测和实际测量值，以最小化均方误差的方式提供对系统状态的最优估计。

核心思想

卡尔曼滤波基于两个基本方程：

1. 状态预测方程（基于系统模型）
2. 测量更新方程（基于实际观测）

它通过以下步骤递归进行：

• 预测：根据系统模型预测下一时刻状态
• 更新：结合观测值修正预测值

数学表述

系统模型

状态方程：
${\mathbf{x}}_k={\mathbf{F}}_k{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{w}}_k$
其中：

• ${\mathbf{x}}_k$：k时刻的系统状态向量（待估计）
• ${\mathbf{F}}_k$：状态转移矩阵
• ${\mathbf{B}}_k$：控制输入矩阵
• ${\mathbf{u}}_k$：控制输入向量
• ${\mathbf{w}}_k$：过程噪声，服从正态分布 $\mathcal{N}(0,{\mathbf{Q}}_k)$

观测方程：
${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$
其中：

• ${\mathbf{z}}_k$：k时刻的观测向量
• ${\mathbf{H}}_k$：观测矩阵
• ${\mathbf{v}}_k$：观测噪声，服从正态分布 $\mathcal{N}(0,{\mathbf{R}}_k)$

滤波算法步骤

1. 预测步骤（先验估计）

状态预测：
${\hat{\mathbf{x}}}_k^{-}={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}^{+}+{\mathbf{B}}_k{\mathbf{u}}_k$

误差协方差预测：
${\mathbf{P}}_k^{-}={\mathbf{F}}_k{\mathbf{P}}_{k-1}^{+}{\mathbf{F}}_k^T+{\mathbf{Q}}_k$

2. 更新步骤（后验估计）

卡尔曼增益计算：
${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}$

状态更新：
${\hat{\mathbf{x}}}_k^{+}={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-})$

协方差更新：
${\mathbf{P}}_k^{+}=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}$

符号说明

• ${\hat{\mathbf{x}}}_k^{-}$：k时刻的先验状态估计（预测值）
• ${\hat{\mathbf{x}}}_k^{+}$：k时刻的后验状态估计（修正值）
• ${\mathbf{P}}_k^{-}$：先验估计误差协方差矩阵
• ${\mathbf{P}}_k^{+}$：后验估计误差协方差矩阵
• ${\mathbf{K}}_k$：卡尔曼增益矩阵
• ${\mathbf{Q}}_k$：过程噪声协方差矩阵
• ${\mathbf{R}}_k$：观测噪声协方差矩阵

直观理解

卡尔曼增益${\mathbf{K}}_k$是关键参数，它决定了我们应该更信任预测值还是观测值：

• 当观测噪声小（${\mathbf{R}}_k\to 0$）时，${\mathbf{K}}_k\to {\mathbf{H}}_k^{-1}$，更信任观测值
• 当预测噪声小（${\mathbf{P}}_k^{-}\to 0$）时，${\mathbf{K}}_k\to 0$，更信任预测值

应用领域

卡尔曼滤波广泛应用于：

• 导航与制导系统（GPS、惯性导航）
• 目标跟踪与雷达系统
• 机器人定位与路径规划
• 经济预测与金融建模
• 信号处理与通信系统

优势与特点

1. 递归性：只需前一时刻的估计值，无需存储全部历史数据
2. 最优性：在线性高斯假设下为最小均方误差估计
3. 计算高效：适合实时应用
4. 适应性：可以通过调整噪声协方差适应不同环境

卡尔曼滤波通过巧妙结合系统动力学模型和实际观测数据，在不确定性环境中提供了对系统状态的最优估计。