扩展卡尔曼滤波算法（EKF）与标准卡尔曼滤波的异同

# 1. 卡尔曼滤波基础概念

卡尔曼滤波是一种用于状态估计的数学方法，它基于线性动态系统和高斯噪声假设。其核心思想是通过融合系统动态模型和传感器测量数据，来估计系统的状态，同时考虑不确定性。卡尔曼滤波包含预测步骤和更新步骤，预测利用系统模型预测状态的变化，更新则利用测量值校正预测结果。常见的应用包括传感器数据融合和机器人定位导航。通过不断迭代预测和更新，卡尔曼滤波能够提高状态估计的精度和稳定性，适用于需要实时动态估计的系统。

# 2. 标准卡尔曼滤波器

在这一章节中，我们将深入介绍标准卡尔曼滤波器的工作原理及其特点。标准卡尔曼滤波器是一种用于估计线性动态系统状态的最优算法，通过利用系统动态方程和观测方程之间的关系，以及误差的高斯分布假设，实现对状态的准确跟踪及噪声的有效滤除。

### 2.1 标准卡尔曼滤波器的工作流程

#### 2.1.1 初始化

标准卡尔曼滤波器的初始化阶段包括设定系统的初始状态估计值和协方差矩阵。首先，根据系统的动态方程和观测方程得到状态空间模型；然后，设定初始状态估计值和协方差矩阵，通常将估计值初始化为系统的初始状态，协方差矩阵初始化为一个较大的值，代表初始不确定性。

#### 2.1.2 预测

在预测步骤中，通过系统的动态方程对当前状态进行预测。利用卡尔曼滤波器的状态方程和误差传播方程，可以得到状态的预测值和预测的协方差矩阵。

#### 2.1.3 更新

更新步骤是通过与观测值进行比较，修正预测值的过程。根据观测方程、预测的状态和协方差矩阵，使用卡尔曼增益来更新状态估计值和协方差矩阵，以使估计值更加接近真实值。

#### 2.1.4 迭代

标准卡尔曼滤波器通过不断的预测和更新步骤迭代进行状态估计，以实现对系统状态的持续跟踪和状态估计值的优化。

### 2.2 标准卡尔曼滤波器特点

#### 2.2.1 线性系统假设

标准卡尔曼滤波器对系统的动态方程和观测方程有严格的线性假设，即系统的演化和观测过程必须是线性的。在非线性情况下，标准卡尔曼滤波器表现效果欠佳，需要使用扩展卡尔曼滤波器等其他算法。

#### 2.2.2 高斯噪声假设

卡尔曼滤波器假设系统的噪声满足高斯分布，这使得卡尔曼滤波器在估计和预测过程中可以利用高斯分布的性质进行最优化。如果系统噪声不符合高斯分布，则卡尔曼滤波器的效果会受到影响。

#### 2.2.3 理想测量情况下效果

在理想情况下，即系统动态方程和观测方程均为线性且系统噪声满足高斯分布时，标准卡尔曼滤波器能够实现最优状态估计，同时保持估计的不确定性最小。然而，在现实应用中，往往会有各种因素影响滤波效果，所以需要根据具体情况选择合适的滤波算法。

在这一章节中深入探讨了标准卡尔曼滤波器的工作原理及特点，了解其关键步骤和适用条件，为接下来介绍扩展卡尔曼滤波器奠定基础。

# 3. 扩展卡尔曼滤波器（EKF）

3.1 EKF算法原理

基于卡尔曼滤波器的线性