卡尔曼滤波

卡尔曼是一种递归的观测算法。基础卡尔曼滤波基于

卡尔曼滤波，用于去除噪声，通过动态融合预测信息和测量信息，实现对系统状态的精准估计。

应用范围：信号处理、导航与定位（如GPS轨迹优化）、机器人控制、传感器数据融合等。

基本工作原理： 1. 用之前数据预测下个数据（基于系统模型的预测） 2. 比对测量值和预测值（计算两者的偏差） 3. 根据比对结果，按比例综合预测值和测量值，得出结果（通过动态权重调整，权衡预测与测量的可信度）

公式1：\(\hat{x_k} = \hat{x_{k-1}} + k_k * (z_{k} - \hat{x_{k-1}})\)

公式2：\(k_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}} + e_{MEA_k}}\)

前置知识

在卡尔曼滤波前，不得不理解一些控制和统计的概念。真的很简单！

状态和状态转移方程

状态，系统某时刻属性。如运动小车的状态，可以是位移、速度、加速度、牵引力等。这些变量综合一起，就是状态，一般表示为向量,记为 \({\mathbf{x}}\)。

状态转移方程，是状态从上一刻到下一刻的关系，体现为函数。转移体现在用上个状态，得出下个状态。如 \({x = x_0 + vt}\) 就是一种简单的状态转移方程。状态转移方程常用矩阵和向量表示，简化计算，标准形式为 \({ \mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k }\)，其中 \({\mathbf{F}_k }\)是状态转移矩阵,\({ \mathbf{B}_k }\)是控制输入矩阵，\({ \mathbf{u}_k }\)是控制输入（如小车的牵引力）。

协方差

协方差，可以衡量两个变量线性关系的方向（和强度）。

定义：有 \(\{ x_n, y_n \}\) 这样的 n 对变量。\(\Delta x_n = (x_n - \bar{x})\)、\(\Delta y_n = (y_n - \bar{y})\) 是n组差值，则协方差\(cov(x,y) = \Sigma (\Delta x_n \cdot \Delta y_n)/(n-1)\)。

• 正负性意义：线性关系方向。大于零，正相关；小于零，负相关。

• （大小意义：线性关系强度。因x,y单位不同，本身难用。使用相关系数\(R = cov(x,y)/(s_x \cdot s_y)\)衡量相关强度更好，s是标准差。）

卡尔曼滤波中，协方差矩阵（\(\mathbf{P}\)）描述状态估计的不确定性。

• 对角线元素：变量自身方差（不确定性大小）
• 非对角线元素：变量间的协方差。

卡尔曼滤波

卡尔曼滤波主要分两个部分，预测和更新，通过递归迭代实现对动态系统状态的持续优化估计。

预测

预测阶段的核心是基于系统模型，用前一时刻的状态估计预测当前时刻的状态，并更新不确定性（协方差矩阵）。

1. 状态预测

根据状态转移方程，用k-1时刻的最优状态估计\({ \hat{\mathbf{x}}_{k-1|k-1} }\)预测k时刻的状态\(\hat{\mathbf{x}}_{k|k-1}\)，公式为：

\(\hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k\)

如，匀速运动小车，k-1 时刻状态为 \({(x_{k-1}, v_{k-1})}\) ，状态转移矩阵 \(\mathbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}\)，\(\Delta t\) 为时间间隔

则预测位移 \(x_{k|k-1} = x_{k-1} + v_{k-1} \cdot \Delta t\)

预测速度 \(v_{k|k-1} = v_{k-1}\)。

1. 协方差预测

预测的状态存在不确定性，需更新协方差矩阵。由于系统运行中存在过程噪声（如路面颠簸对小车运动的干扰，记为\(\mathbf{Q}_k\)），协方差矩阵会增大，公式为：

\(\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T + \mathbf{Q}_k\)

其中\(\mathbf{F}_k^T\)是\(\mathbf{F}_k\)的转置矩阵，\(\mathbf{P}_{k-1|k-1}\)是k-1时刻的最优协方差矩阵。

更新

更新阶段的核心是结合当前时刻的测量值，修正预测结果，得到更优的状态估计，并更新不确定性。

1. 计算测量残差 测量残差（innovation）是实际测量值与预测值的偏差，反映预测与现实的差距，公式：

\(\mathbf{y}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1}\)

• \(\mathbf{z}_k\) 是 k 时刻的测量值

• \(\mathbf{H}_k\) 是测量矩阵，将状态向量映射到测量向量（如仅测量位移时，\(\mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}\)）

• 计算卡尔曼增益

卡尔曼增益（\(\mathbf{K}_k\)）是更新权重，权衡预测值（\({ \mathbf{P}_{k|k-1} }\)）和测量值（\(\mathbf{R}_k\)）的可信度。公式：

\(\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)^{-1}\)

• 测量噪声（\(\mathbf{R}_k\)）小，卡尔曼增益大，更信任测量值

• 过程噪声（\(\mathbf{Q}_k\)）小，卡尔曼增益小，更信任预测值

• 更新状态估计

用卡尔曼增益和测量残差，修正预测状态，得到k时刻状态估计：

\(\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \mathbf{y}_k\)

1. 更新协方差矩阵

修正后的状态不确定性（协方差矩阵）为：

\(\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}\)

其中 \(\mathbf{I}\) 是单位矩阵。更新后，协方差矩阵通常会比预测阶段更小，状态估计不确定性降低。

通过“预测-更新”的循环迭代，卡尔曼滤波能输出精准的状态估计。