zy123/md_files
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
md_files/科研/卡尔曼滤波.md

16 KiB
Raw Permalink Blame History

卡尔曼滤波

卡尔曼滤波（Kalman Filter）是一种用于线性动态系统状态估计的递归最优滤波算法，它在噪声环境下对系统状态进行估计，并常用于目标跟踪、导航和控制等领域。

卡尔曼滤波假设系统可以用状态空间模型描述，模型包括两个部分：

  • 状态转移模型：描述系统状态如何从上一时刻转移到当前时刻。
  • 测量模型：描述通过传感器获得的测量值与系统状态之间的关系。

这两个模型中均包含随机噪声，分别记为过程噪声和测量噪声。卡尔曼滤波的目标就是在已知这些噪声统计特性的前提下，利用当前和过去的测量值来对系统状态进行最优估计。

引入

image-20240311130512387

image-20240311130359589

image-20240311130828892

公式

状态转移模型

设系统的状态向量为 $\mathbf{x}_k$，控制输入为 $\mathbf{u}_k$，过程噪声为 $\mathbf{w}_k$（假设均值为0，协方差矩阵为 $\mathbf{Q}$，维度和状态向量一致），状态转移模型可写为：


\mathbf{x}_k = \mathbf{A} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}

其中：

  • \mathbf{A} 是状态转移矩阵，
  • \mathbf{B} 是控制输入矩阵。

测量模型

设测量向量为 $\mathbf{z}_k$，测量噪声为 $\mathbf{v}_k$（假设均值为0，协方差矩阵为 $\mathbf{R}$），测量模型为：


\mathbf{z}_k = \mathbf{H} \mathbf{x}_k + \mathbf{v}_k

其中：

  • \mathbf{H} 是测量矩阵。

这里是真实状态、真实测量、过程噪声、测量噪声。在卡尔曼滤波的预测和更新阶段中，只需在每个时刻把新测得的 z_k （再加上可用的控制输入 $u_{k-1}$）喂进去，滤波器就会自动递推状态估计。

递归过程

卡尔曼滤波的递归过程主要分为两大步：预测（Prediction） 和 更新（Update）。

注意：$\hat{\mathbf{x}}_k^-$右上角的'-'符号是区分预测状态和更新后的状态。

预测步骤

  1. 状态预测：

    利用系统的状态转移模型，将上一次的状态估计 \hat{\mathbf{x}}_{k-1} 通过转移矩阵 $\mathbf{A}$（和控制输入 $\mathbf{B} \mathbf{u}_{k-1}$）预测到当前时刻的状态：

    
\hat{\mathbf{x}}_k^- = \mathbf{A} \hat{\mathbf{x}}_{k-1} + \mathbf{B} \mathbf{u}_{k-1}

    这里 \hat{\mathbf{x}}_k^- 称为先验状态估计，它反映了系统在没有新测量数据情况下的预期状态。

  2. 协方差预测： 同时，将上一次状态的不确定性（协方差矩阵 $\mathbf{P}_{k-1}$）传播到当前时刻，并加上过程噪声 \mathbf{Q} 的影响：

    
\mathbf{P}_k^- = \mathbf{A} \mathbf{P}_{k-1} \mathbf{A}^\mathrm{T} + \mathbf{Q}

    这个预测协方差反映了预测状态的置信程度，不确定性通常会因过程噪声的加入而增大。

更新步骤

当时刻 k 新的测量值 \mathbf{z}_k 到达时，我们使用它来校正预测结果。

  1. 卡尔曼增益的计算： 卡尔曼增益 \mathbf{K}_k 衡量了预测的不确定性与测量不确定性之间的权衡。计算公式为：

    
\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}^\mathrm{T} \left(\mathbf{H} \mathbf{P}_k^- \mathbf{H}^\mathrm{T} + \mathbf{R}\right)^{-1}

    当预测的置信度较低（$\mathbf{P}_k^-$较大）时，卡尔曼增益较大，说明更多地信任测量值；反之，则更多地依赖预测值。

  2. 状态更新： 根据卡尔曼增益修正先验状态，将测量的偏差信息（即测量值与预测值之间的差异，也叫创新）加权融合：

    
\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \left(\mathbf{z}_k - \mathbf{H} \hat{\mathbf{x}}_k^- \right)

    这个更新后的状态 \hat{\mathbf{x}}_k 就是当前时刻的后验状态估计，它综合了预测和测量两方面的信息。

  3. 协方差更新： 更新后的协方差表示在新的测量信息下的不确定性：

    
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}) \mathbf{P}_k^-

    一般来说，经过更新后，状态的不确定性会降低（即协方差矩阵的数值减小）。

疑问：

状态转移模型：为什么包含噪声？

状态转移模型描述的是系统状态的真实动态行为，它是一个理论模型，表示状态如何从 \mathbf{x}_{k-1} 演化到 $\mathbf{x}k$。由于现实系统存在不确定性（如建模误差、外部扰动等），这些无法精确建模的部分被抽象为**过程噪声 $\mathbf{w}{k-1}$**。因此，模型写作：


\mathbf{x}_k = \mathbf{A} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}

状态预测：为什么不带噪声？

在卡尔曼滤波的预测步骤中，我们计算的是状态的期望值（即最优估计），而非真实状态本身。由于噪声 \mathbf{w}_{k-1} 的均值为零，它在预测时的期望贡献为零：


\mathbb{E}[\mathbf{x}_k] = \mathbf{A} \mathbb{E}[\mathbf{x}_{k-1}] + \mathbf{B} \mathbf{u}_{k-1} + \mathbb{E}[\mathbf{w}_{k-1}] = \mathbf{A} \hat{\mathbf{x}}_{k-1} + \mathbf{B} \mathbf{u}_{k-1}

协方差预测：噪声的体现

虽然噪声的均值在状态预测中被忽略，但其随机性会导致不确定性累积。因此，协方差预测公式中显式加入了 $\mathbf{Q}$：


\mathbf{P}_k^- = \mathbf{A} \mathbf{P}_{k-1} \mathbf{A}^\mathrm{T} + \mathbf{Q}

扩展卡尔曼滤波

扩展卡尔曼滤波（Extended Kalman Filter，简称 EKF）是一种针对非线性系统状态估计问题的滤波方法。传统的卡尔曼滤波要求系统的状态转移和观测模型都是线性的，而在实际问题中，很多系统往往存在非线性特性。

EKF 的核心思想就是对非线性模型进行局部线性化，然后在线性化后的模型上直接套用标准卡尔曼滤波（KF）的预测和更新公式。

  1. 非线性系统模型
    假设系统的状态转移和观测模型为非线性的：

    • 状态转移模型：

      
\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_{k-1}

    • 观测模型：

      
\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k

      其中，f(\cdot) 和 h(\cdot) 为非线性函数，\mathbf{w}_{k-1} 和 \mathbf{v}_k 分别表示过程噪声和测量噪声（均假设为零均值高斯噪声）。

  2. 线性化
    为了使用卡尔曼滤波方法，扩展卡尔曼滤波需要对非线性函数进行局部线性化。具体做法是使用泰勒展开在当前状态估计附近进行一阶近似，计算函数的雅可比矩阵：

    • 状态转移函数 f 的雅可比矩阵：

      
F_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}_{k-1}, \mathbf{u}=\mathbf{u}_{k-1}}

    • 观测函数 h 的雅可比矩阵：

      
H_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}_k^-}

  3. 滤波过程
    扩展卡尔曼滤波的递归过程与标准卡尔曼滤波类似，但在每一步都需要用雅可比矩阵替换原来的线性模型矩阵：

    • 预测步骤：

      • 状态预测：

        
\hat{\mathbf{x}}_k^- = f(\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1})

      • 协方差预测：

        
\mathbf{P}_k^- = F_k \mathbf{P}_{k-1} F_k^\mathrm{T} + \mathbf{Q}

        这里 F_k 是在 \hat{\mathbf{x}}_{k-1} 处计算得到的雅可比矩阵。

    • 更新步骤：

      • 计算卡尔曼增益：

        
\mathbf{K}_k = \mathbf{P}_k^- H_k^\mathrm{T} \left(H_k \mathbf{P}_k^- H_k^\mathrm{T} + \mathbf{R}\right)^{-1}

      • 状态更新：

        
\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \left(\mathbf{z}_k - h(\hat{\mathbf{x}}_k^-)\right)

      • 协方差更新：

        
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k H_k) \mathbf{P}_k^-

通过这样的线性化步骤，EKF 能够对非线性系统进行状态估计，虽然由于线性化近似可能带来一定误差，但在大多数情况下能达到较好的效果。

雅各比矩阵定义

雅可比矩阵（Jacobian Matrix）是一个多变量函数各个分量对各个变量的偏导数组成的矩阵。它反映了在某一点处函数的局部线性化近似，也就是该函数在这一点的“导数”信息。在扩展卡尔曼滤波中，为了对非线性状态转移函数 f(\mathbf{x}, \mathbf{u}) 或观测函数 h(\mathbf{x}) 进行线性化，我们需要计算它们在当前估计点的雅可比矩阵。

示例 1：状态转移函数的雅可比矩阵

假设系统的状态为 $\mathbf{x} = \begin{bmatrix} x_1 \ x_2 \end{bmatrix}$（例如，x_1 表示位置，x_2 表示速度），状态转移函数定义为：


f(\mathbf{x}) =
\begin{bmatrix}
f_1(x_1, x_2) \\
f_2(x_1, x_2)
\end{bmatrix}
=
\begin{bmatrix}
x_1 + x_2 + 0.1 x_1^2 \\
x_2 + 0.05 x_1
\end{bmatrix}

这里函数中的非线性项为 0.1 x_1^2 和 $0.05 x_1$。

求雅可比矩阵

雅可比矩阵 F 是一个 2 \times 2 矩阵，其中每个元素为：


F_{ij} = \frac{\partial f_i}{\partial x_j}

计算各个偏导数：

  1. 对 $f_1(x_1, x_2) = x_1 + x_2 + 0.1 x_1^2$：

    • \frac{\partial f_1}{\partial x_1} = 1 + 0.2x_1
    • \frac{\partial f_1}{\partial x_2} = 1

  2. 对 $f_2(x_1, x_2) = x_2 + 0.05 x_1$：

    • \frac{\partial f_2}{\partial x_1} = 0.05
    • \frac{\partial f_2}{\partial x_2} = 1

因此，雅可比矩阵为：


F = \begin{bmatrix}
1 + 0.2x_1 & 1 \\
0.05 & 1
\end{bmatrix}

示例 2：观测函数的雅可比矩阵

假设观测函数为：


h(\mathbf{x}) =
\begin{bmatrix}
h_1(x_1, x_2) \\
h_2(x_1, x_2)
\end{bmatrix}
=
\begin{bmatrix}
\sqrt{x_1} \\
x_2
\end{bmatrix}

这里假设传感器对位置进行非线性测量（取平方根），而速度直接测量。

求雅可比矩阵

计算各个偏导数：

  1. 对 $h_1(x_1, x_2) = \sqrt{x_1}$：

    • \frac{\partial h_1}{\partial x_1} = \frac{1}{2\sqrt{x_1}}
    • $\frac{\partial h_1}{\partial x_2} = 0$（因为 h_1 与 x_2 无关）

  2. 对 $h_2(x_1, x_2) = x_2$：

    • \frac{\partial h_2}{\partial x_1} = 0
    • \frac{\partial h_2}{\partial x_2} = 1

因此，雅可比矩阵为：


H = \begin{bmatrix}
\frac{1}{2\sqrt{x_1}} & 0 \\
0 & 1
\end{bmatrix}

无迹卡尔曼(UKF)

UKF 具体步骤（分步解析）

符号 含义 维度
\mathbf{x} 系统状态向量 n \times 1
P 状态协方差矩阵 n \times n
\mathbf{z} 观测向量 m \times 1
f(\cdot) 非线性状态转移函数 -
h(\cdot) 非线性观测函数 -
Q 过程噪声协方差 n \times n
R 观测噪声协方差 m \times m
\mathcal{X} Sigma点集合 n \times (2n+1)
W^{(m)} 均值权重 1 \times (2n+1)
W^{(c)} 协方差权重 1 \times (2n+1)
\alpha, \beta, \kappa UKF调参参数（控制Sigma点分布） 标量

建模：

x_k = f(x_{k-1}) + w_k y_k = h\left(x_k\right) + v_k

Step 1: 生成Sigma点（确定性采样）

目的：根据当前状态均值和协方差，生成一组代表状态分布的采样点。
公式：


\begin{aligned}
\mathcal{X}_0 &= \hat{\mathbf{x}}_{k-1|k-1} \\
\mathcal{X}_i &= \hat{\mathbf{x}}_{k-1|k-1} + \left( \sqrt{(n+\lambda) P_{k-1|k-1}} \right)_i \quad (i=1,\dots,n) \\
\mathcal{X}_{i+n} &= \hat{\mathbf{x}}_{k-1|k-1} - \left( \sqrt{(n+\lambda) P_{k-1|k-1}} \right)_i \quad (i=1,\dots,n)
\end{aligned}

符号说明：

  • $ \sqrt{(n+\lambda) P} $：协方差矩阵的平方根（如Cholesky分解）。
  • \left( \sqrt{(n+\lambda) P} \right)_i 表示平方根矩阵的第 i 列。
  • $ \lambda = \alpha^2 (n + \kappa) - n ：缩放因子（ \alpha 控制分布范围，通常取1e-3； \kappa $通常取0）。
  • 为什么是 2n+1 个点？1个中心点 + $ 2n $个对称点，覆盖状态空间的主要方向。

示例：

假设状态 $ \mathbf{x} = [x, y]^T ， n = 2 ， P = \begin{bmatrix} 4 & 0 \ 0 & 1 \end{bmatrix} ， \lambda = 0 $：

  1. 计算平方根矩阵（Cholesky分解）：

    
\sqrt{(n+\lambda) P} = \sqrt{2} \cdot \begin{bmatrix} 2 & 0 \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} 2.828 & 0 \\ 0 & 1.414 \end{bmatrix}

  2. 生成 Sigma 点：

    
\begin{aligned}
\mathcal{X}_0 &= \hat{\mathbf{x}} \\
\mathcal{X}_1 &= \hat{\mathbf{x}} + [2.828, 0]^T = [\hat{x} + 2.828, \hat{y}] \\
\mathcal{X}_2 &= \hat{\mathbf{x}} + [0, 1.414]^T = [\hat{x}, \hat{y} + 1.414] \\
\mathcal{X}_3 &= \hat{\mathbf{x}} - [2.828, 0]^T = [\hat{x} - 2.828, \hat{y}] \\
\mathcal{X}_4 &= \hat{\mathbf{x}} - [0, 1.414]^T = [\hat{x}, \hat{y} - 1.414] \\
\end{aligned}

Step 2: 计算Sigma点权重

目的：为每个Sigma点分配权重，用于后续计算均值和协方差。
公式：


\begin{aligned}
W_0^{(m)} &= \frac{\lambda}{n + \lambda} \quad &\text{(中心点均值权重)} \\
W_0^{(c)} &= \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta) \quad &\text{(中心点协方差权重)} \\
W_i^{(m)} = W_i^{(c)} &= \frac{1}{2(n + \lambda)} \quad (i=1,\dots,2n) \quad &\text{(对称点权重)}
\end{aligned}

符号说明：

  • $ \beta $：高阶矩调节参数（高斯分布时取2最优）。
  • 权重作用：中心点通常权重较大，对称点权重均等。

Step 3: 预测步骤（时间更新）

目的：将Sigma点通过非线性状态方程传播，计算预测状态和协方差。
子步骤：

  1. 传播Sigma点：

    
\mathcal{X}_{i,k|k-1}^* = f(\mathcal{X}_{i,k-1}, \mathbf{u}_{k-1}), \quad i=0,1,...,2n

    （每个Sigma点独立通过 f(\cdot) 计算）

  2. 计算预测均值和协方差：

    
\hat{\mathbf{x}}_{k|k-1} = \sum_{i=0}^{2n} W_i^{(m)} \mathcal{X}_{i,k|k-1}^*

    
P_{k|k-1} = \sum_{i=0}^{2n} W_i^{(c)} \left( \mathcal{X}_{i,k|k-1}^* - \hat{\mathbf{x}}_{k|k-1} \right) \left( \mathcal{X}_{i,k|k-1}^* - \hat{\mathbf{x}}_{k|k-1} \right)^T + Q_k

    符号说明：

    • $\mathcal{X}_{k-1}$：上一时刻生成的Sigma点集合（$2n+1$个点）

    • $\mathcal{X}_{k|k-1}^*$：通过状态方程传播后的Sigma点集合

    • $ Q_k $：过程噪声（表示模型不确定性）。

Step 4: 观测更新（测量更新）

目的：将预测的Sigma点通过观测方程传播，计算卡尔曼增益并更新状态。
子步骤：

  1. 生成观测Sigma点：

    
\mathcal{Z}_{i,k|k-1} = h(\mathcal{X}_{i,k|k-1}^*), \quad i=0,...,2n

  2. 计算观测预测统计量：

    
\hat{\mathbf{z}}_{k|k-1} = \sum_{i=0}^{2n} W_i^{(m)} \mathcal{Z}_{i,k|k-1}

    
P_{z_k z_k} = \sum_{i=0}^{2n} W_i^{(c)} \left( \mathcal{Z}_{i,k|k-1} - \hat{\mathbf{z}}_{k|k-1} \right) \left( \mathcal{Z}_{i,k|k-1} - \hat{\mathbf{z}}_{k|k-1} \right)^T + R_k

    
P_{x_k z_k} = \sum_{i=0}^{2n} W_i^{(c)} \left( \mathcal{X}_{i,k|k-1}^* - \hat{\mathbf{x}}_{k|k-1} \right) \left( \mathcal{Z}_{i,k|k-1} - \hat{\mathbf{z}}_{k|k-1} \right)^T

    符号说明：

    • $ P_{z_k z_k} ：观测自协方差（含噪声 R_k $）。
    • $ P_{x_k z_k} $：状态-观测互协方差。

  3. 计算卡尔曼增益和更新状态：

    
K_k = P_{x_k z_k} P_{z_k z_k}^{-1}

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

    
P_{k|k} = P_{k|k-1} - K_k P_{z_k z_k} K_k^T