255 lines
8.9 KiB
Markdown
255 lines
8.9 KiB
Markdown
# 卡尔曼滤波
|
||
|
||
卡尔曼滤波（Kalman Filter）是一种用于线性动态系统状态估计的递归最优滤波算法，它在**噪声环境**下对系统状态进行估计，并常用于目标跟踪、导航和控制等领域。
|
||
|
||
卡尔曼滤波假设系统可以用状态空间模型描述，模型包括两个部分：
|
||
|
||
- **状态转移模型**：描述系统状态如何从上一时刻转移到当前时刻。
|
||
- **测量模型**：描述通过传感器获得的测量值与系统状态之间的关系。
|
||
|
||
这两个模型中均包含随机噪声，分别记为过程噪声和测量噪声。卡尔曼滤波的目标就是在已知这些噪声统计特性的前提下，利用当前和过去的测量值来对系统状态进行最优估计。
|
||
|
||
## 引入
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
## 公式
|
||
|
||
**状态转移模型**
|
||
|
||
设系统的状态向量为 $\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}$ 是测量矩阵。
|
||
|
||
|
||
|
||
## 递归过程
|
||
|
||
卡尔曼滤波的递归过程主要分为两大步：**预测（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^-
|
||
$$
|
||
一般来说，经过更新后，状态的不确定性会降低（即协方差矩阵的数值减小）。
|
||
|
||
|
||
|
||
|
||
|
||
# 扩展卡尔曼滤波
|
||
|
||
扩展卡尔曼滤波（Extended Kalman Filter，简称 EKF）是一种针对非线性系统状态估计问题的滤波方法。传统的卡尔曼滤波要求系统的状态转移和观测模型都是线性的，而在实际问题中，很多系统往往存在非线性特性。EKF 的核心思想就是对非线性模型进行局部线性化，然后在此基础上应用卡尔曼滤波的递归估计方法。
|
||
|
||
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}
|
||
$$
|
||
|
||
###
|