卡尔曼滤波综述——稍微做下笔记（3）

卡尔曼滤波是一种经典的最优估计方法，用于将测量值和预测值进行融合，以获得更准确的状态估计。根据应用场景和问题要求，卡尔曼滤波可以分为以下几种类型：

1. 线性卡尔曼滤波（Linear Kalman Filter）：适用于线性系统和线性观测模型的状态估计。系统动态和观测模型均为线性关系，噪声也是高斯白噪声。这是最经典的卡尔曼滤波算法，适用于许多实际问题的估计。

2. 扩展卡尔曼滤波（Extended Kalman Filter，EKF）：用于非线性系统和非线性观测模型的状态估计。EKF通过在非线性模型上进行一阶泰勒展开，近似为线性模型，并通过线性卡尔曼滤波的方法进行估计。适用于许多非线性问题，但需要对模型进行线性化，因此在某些情况下可能不够精确。

3. 无迹卡尔曼滤波（Unscented Kalman Filter，UKF）：也用于非线性系统和非线性观测模型的状态估计。相比于EKF，UKF通过选择一组特定的采样点（Sigma Points）来对非线性函数进行估计，避免了线性化过程，提高了估计的准确性。

4. 类卡尔曼滤波（Kalman-like Filter）：包括EKF、UKF等算法，是对标准卡尔曼滤波的改进和扩展；适用于非线性系统和非高斯噪声情况，可以处理非线性观测函数；提供更准确和稳健的状态估计，适用于更广泛的实际应用。

5. 平滑处理：除了滤波，卡尔曼滤波还可以进行平滑处理，即对所有的观测数据进行联合估计，以获得更准确的状态估计。常见的平滑方法包括卡尔曼平滑（Kalman Smoother）和无迹卡尔曼平滑（Unscented Kalman Smoother，UKS）。

总的来说，卡尔曼滤波是一种强大的状态估计方法，适用于很多实际问题，但在应用中需要根据具体的问题要求和系统特性选择合适的滤波方法。对于非线性问题，可以考虑使用扩展卡尔曼滤波或无迹卡尔曼滤波，以获得更精确的估计结果。同时，需要对系统模型和观测模型进行合理的建模，以提高滤波器的性能。

线性卡尔曼滤波

线性卡尔曼滤波（Linear Kalman Filter）是一种递归的状态估计算法，用于估计具有线性动态模型和线性观测函数的动态系统状态。它是由R.E. Kalman于1960年提出的，广泛应用于控制、导航、信号处理、机器学习等领域。

线性卡尔曼滤波的主要思想是根据系统的动态模型和观测函数，在每个时间步骤中对系统的状态进行预测和更新。它基于贝叶斯滤波理论，将先验信息和测量信息相结合，得到最优的状态估计值和估计误差协方差矩阵。

线性卡尔曼滤波的一般步骤如下：

1. 系统建模：首先需要对系统进行建模，包括状态转移矩阵A、控制输入矩阵B、过程噪声协方差矩阵Q、观测矩阵H、观测噪声协方差矩阵R等。这些参数描述了系统的动态演化和观测过程。

2. 初始状态估计：在滤波开始时，需要提供初始状态估计值x_hat和初始协方差矩阵P。通常情况下，初始估计值和协方差矩阵可以通过系统先验知识或历史测量数据得到。

3. 预测步骤（时间更新）：在每个时间步骤中，首先根据系统的动态模型进行状态预测。利用状态转移矩阵A和控制输入矩阵B，计算预测状态x_pred和预测协方差矩阵P_pred。

4. 更新步骤（测量更新）：接下来，根据观测数据进行状态更新。通过观测矩阵H将预测状态映射到观测空间，得到预测观测值z_pred。然后，计算观测残差y，即实际观测值z与预测观测值z_pred之间的差异。

5. 计算卡尔曼增益：卡尔曼增益K用于融合先验估计和测量信息。它是一个权重参数，用于平衡先验信息和测量信息的可靠性。卡尔曼增益的计算依赖于预测协方差矩阵P_pred和观测噪声协方差矩阵R。

6. 更新状态估计：利用卡尔曼增益和观测残差y，对预测状态进行修正，得到更新后的状态估计x_hat和更新后的协方差矩阵P。

7. 递归迭代：线性卡尔曼滤波是一个递归过程，通过不断的预测和更新步骤，可以在每个时间步骤中获得最优的状态估计值和估计误差协方差矩阵。

线性卡尔曼滤波在实际应用中广泛使用，特别适用于那些具有线性动态模型和线性观测函数的系统。它具有高效、精确、递归和自适应的特点，在状态估计问题中发挥着重要的作用。然而，对于非线性系统和非线性观测函数，线性卡尔曼滤波的效果可能会受限。在这种情况下，可以使用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）等方法来处理非线性问题。

扩展卡尔曼滤波

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是对非线性系统和非线性观测模型进行状态估计的一种卡尔曼滤波扩展方法。EKF通过在非线性模型上进行一阶泰勒展开，近似为线性模型，并通过线性卡尔曼滤波的方法进行估计。这种方法在许多非线性问题中表现良好，但需要对模型进行线性化，因此在某些情况下可能不够精确。

EKF的基本步骤如下：

1. 状态传播（预测）步骤：根据系统的状态转移方程，对当前状态进行预测。假设系统的状态转移方程为 x_k = f(x_{k-1}, u_{k-1})，其中 x_k 是状态向量，u_{k-1} 是控制输入。通过将状态转移方程在上一时刻的状态和控制输入处进行一阶泰勒展开，可以得到线性近似：x_k ≈ f(x_{k-1}, u_{k-1}) + F_{k-1} \cdot (x_{k-1} - \hat{x}_{k-1}) + B_{k-1} \cdot (u_{k-1} - \hat{u}_{k-1})，其中 F_{k-1} 和 B_{k-1} 是状态转移方程在当前状态和控制输入处的雅可比矩阵，\hat{x}_{k-1} 和 \hat{u}_{k-1} 是上一时刻的状态和控制输入的估计值。

2. 预测协方差：通过线性化的状态转移方程，可以得到状态预测的协方差矩阵 P_k = F_{k-1} \cdot P_{k-1} \cdot F_{k-1}^T + Q_{k-1}，其中 P_{k-1} 是上一时刻的状态协方差矩阵，Q_{k-1} 是过程噪声的协方差矩阵。

3. 测量更新步骤：接收观测值 z_k，并通过观测模型将预测值与观测值进行比较。假设观测模型为 z_k = h(x_k)，其中 h(x_k) 是非线性函数。通过在预测值处进行一阶泰勒展开，可以得到线性近似：z_k ≈ h(x_k) + H_k \cdot (x_k - \hat{x}_k)，其中 H_k 是观测模型在预测值处的雅可比矩阵，\hat{x}_k 是状态预测的估计值。

4. 卡尔曼增益计算：根据预测协方差和观测模型的雅可比矩阵，可以计算卡尔曼增益 K_k = P_k \cdot H_k^T \cdot (H_k \cdot P_k \cdot H_k^T + R_k)^{-1}，其中 R_k 是观测噪声的协方差矩阵。

5. 状态更新：将卡尔曼增益应用于观测残差，得到状态更新：\hat{x}_k = \hat{x}_k + K_k \cdot (z_k - h(\hat{x}_k))。

6. 更新协方差：根据卡尔曼增益和观测模型的雅可比矩阵，可以更新状态估计的协方差矩阵：P_k = (I - K_k \cdot H_k) \cdot P_k。

以上步骤即为EKF的基本算法，它在处理非线性问题时利用了一阶泰勒展开的线性近似，通过迭代预测和更新步骤，可以逐步优化状态估计值。需要注意的是，EKF的性能受到模型线性化的影响，因此在某些情况下可能会出现估计不准确的问题。为了更好地应对非线性问题，还有其他非线性滤波方法，如无迹卡尔曼滤波（UKF）等，可以根据具体问题选择合适的滤波算法。

无迹卡尔曼滤波

无迹卡尔曼滤波（Unscented Kalman Filter，UKF）是对非线性系统进行状态估计的一种卡尔曼滤波扩展方法，它通过使用无迹变换（Unscented Transform）来处理非线性问题，避免了对系统模型进行线性化。相比于扩展卡尔曼滤波（EKF），UKF在某些非线性问题上表现更好，特别是在高维状态空间和大量观测噪声的情况下。

UKF的基本步骤如下：

1. 状态传播（预测）步骤：根据系统的状态转移方程，对当前状态进行预测。与EKF不同，UKF使用无迹变换来生成状态传播时的采样点，而不是进行一阶泰勒展开。具体来说，UKF通过在当前状态处取一些特定的采样点，称为Sigma点，来代表整个状态分布。Sigma点的选择是通过计算系统状态和协方差的Cholesky分解来完成。然后，通过将状态转移方程应用于每个Sigma点，得到预测的状态和协方差。

2. 预测协方差：通过Sigma点的状态预测值，计算预测的协方差矩阵。在这一步骤中，同样使用无迹变换来计算协方差矩阵，避免了对非线性函数进行线性化。

3. 测量更新步骤：接收观测值，并通过观测模型将预测值与观测值进行比较。与EKF类似，UKF同样使用无迹变换来处理观测模型的非线性性。通过在预测值处取Sigma点，并将其通过观测模型转换为测量空间，得到预测的观测值和协方差。

4. 卡尔曼增益计算：根据预测的状态协方差、预测的观测协方差和协方差之间的交叉协方差，计算卡尔曼增益。

5. 状态更新：将卡尔曼增益应用于观测残差，得到状态更新。

6. 更新协方差：根据卡尔曼增益和协方差之间的交叉协方差，更新状态估计的协方差矩阵。

通过使用无迹变换，UKF能够更准确地估计非线性系统的状态，避免了EKF中对系统模型进行线性化可能引入的误差。然而，UKF也有一些缺点，如计算复杂度较高，对于高维状态空间和大量观测噪声的问题，计算量会增加。因此，在实际应用中需要权衡计算复杂度和估计精度，并选择合适的滤波方法。

类卡尔曼滤波

类卡尔曼滤波（Kalman Filter with Augmentation）是对传统卡尔曼滤波的一种改进和扩展。它的主要目的是在传统卡尔曼滤波的基础上，通过增加状态和测量向量的维度，使得卡尔曼滤波更适用于复杂的系统，特别是那些具有非线性动态模型和非线性观测函数的系统。

类卡尔曼滤波的一般步骤如下：

1. 系统建模：类卡尔曼滤波同样需要对系统进行建模，包括状态转移矩阵A、控制输入矩阵B、过程噪声协方差矩阵Q、观测矩阵H、观测噪声协方差矩阵R等。

2. 初始状态估计：与传统卡尔曼滤波相同，需要提供初始状态估计值x_hat和初始协方差矩阵P。

3. 预测步骤（时间更新）：类卡尔曼滤波的预测步骤与传统卡尔曼滤波一样，根据状态转移矩阵A和控制输入预测下一时刻的状态估计x_pred和协方差矩阵P_pred。

4. 增广步骤：在类卡尔曼滤波中，需要增加状态向量x和观测向量z的维度，即引入增广状态向量x_aug和增广观测向量z_aug。这是为了处理复杂系统中可能存在的非线性动态模型和非线性观测函数。

5. 状态增广：将原始状态向量x和控制输入向量u扩展成增广状态向量x_aug，即 x_aug = [x; u]。

6. 观测增广：将原始观测向量z扩展成增广观测向量z_aug，即 z_aug = [z; u']。其中，u'是一个增广向量，通常可以设置为零向量。

7. 预测增广步骤：使用状态转移矩阵A_aug和增广状态向量x_aug进行预测，得到增广状态估计x_pred_aug和协方差矩阵P_pred_aug。

8. 计算雅可比矩阵：对于非线性动态模型和观测函数，需要计算其对应的雅可比矩阵，用于线性化这些非线性函数。

9. 更新步骤（测量更新）：类卡尔曼滤波的更新步骤与传统卡尔曼滤波类似，根据观测噪声协方差矩阵R和预测增广观测值h(x_pred_aug)进行测量更新。通过计算卡尔曼增益K_aug，并根据观测数据z_aug，更新增广状态估计x_hat_aug和协方差矩阵P_aug。

10. 提取估计结果：从增广状态估计x_hat_aug中提取出原始状态估计x_hat和协方差矩阵P，得到最终的状态估计结果。

类卡尔曼滤波通过增加状态和观测向量的维度，使得卡尔曼滤波能够更好地应用于非线性和复杂的系统。它适用于那些需要处理高度非线性动态模型和观测函数的实际问题，比如机器人定位与导航等。

平滑处理

在卡尔曼滤波中，平滑处理是对已经观测到的数据进行再处理，以获得更准确的状态估计。平滑处理是在整个时间序列上进行的，使用过去的所有观测数据来优化状态的估计，包括之前已经进行过预测和更新的数据。与滤波不同，滤波是根据当前时刻的观测值和之前的预测进行状态估计，而平滑处理则是利用全部已知的观测值对状态进行估计。

卡尔曼滤波中的平滑处理通常采用回溯平滑技术，也称为递归平滑或后向平滑。它基于递归计算过程，从最后一个观测点开始，向前逐步计算每个时刻的状态估计，并逐步向前传播。平滑处理的基本思想是利用过去时刻的状态估计来改进当前时刻的状态估计，以及通过观测值的信息来对过去时刻的状态进行修正。

回溯平滑的基本步骤如下：

1. 初始化：从最后一个观测点开始，用滤波得到的最终状态估计值作为初始状态估计。

2. 平滑步骤：从最后一个观测点开始，逐步向前进行平滑处理。在每个时刻，通过已经计算得到的当前状态估计和下一个时刻的状态估计，结合过去的观测数据，计算平滑后的状态估计和协方差。

3. 平滑协方差：在每个时刻，通过平滑后的状态估计和协方差，计算平滑后的协方差。

通过回溯平滑技术，可以在整个时间序列上获得更准确的状态估计。平滑处理的结果通常比滤波处理更稳定，对观测误差的影响更小，因为它利用了整个时间序列的信息来进行优化。然而，回溯平滑的计算复杂度较高，特别是在时间序列较长或状态空间较大的情况下，计算量会显著增加。

总的来说，平滑处理是卡尔曼滤波的一个重要扩展，它通过回溯整个时间序列来优化状态估计，提供了更准确和稳定的状态估计结果。但同时，由于计算复杂度的增加，需要在实际应用中权衡计算成本和估计精度，选择合适的处理方法。

非线性观测函数

在卡尔曼滤波中，观测函数通常被假设为线性函数，这使得卡尔曼滤波在处理线性系统方面非常高效和有效。然而，在实际应用中，有些系统的观测函数是非线性的，即状态与观测之间的关系不能用简单的线性方程描述。对于这种情况，传统的卡尔曼滤波无法直接应用，需要采用非线性观测函数的扩展版本，例如扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。

在非线性观测函数中，观测向量z和状态向量x之间的关系被表示为一个非线性函数h(x)，即：

z = h(x) + w

其中，z是观测向量，x是状态向量，h(x)是非线性观测函数，w是观测噪声。由于h(x)是非线性函数，无法直接使用卡尔曼滤波的线性观测更新步骤。

在扩展卡尔曼滤波（EKF）中，通过泰勒级数展开将非线性观测函数近似线性化，从而使得卡尔曼滤波的线性观测更新步骤可以应用于非线性系统。具体步骤如下：

1. 预测步骤（时间更新）：与传统卡尔曼滤波相同，根据状态转移矩阵和控制输入预测下一时刻的状态估计x_pred和协方差矩阵P_pred。

2. 计算雅可比矩阵：对非线性观测函数h(x)进行泰勒级数展开，得到其雅可比矩阵H，该矩阵用于近似线性化观测函数。

3. 更新步骤（测量更新）：利用观测噪声协方差矩阵R和预测观测值h(x_pred)进行测量更新。通过计算卡尔曼增益K，并根据观测数据z，更新状态估计x_hat和协方差矩阵P。

EKF的优点在于相对简单且易于实现，适用于较小的非线性度的系统。然而，当非线性度较高时，EKF可能会出现不稳定和发散的问题，因为它是基于一阶泰勒展开，只能近似线性化观测函数。

无迹卡尔曼滤波（UKF）是对EKF的改进，它通过使用一组特殊选择的采样点来近似非线性观测函数，避免了一阶泰勒展开的不足。UKF通过一系列的Sigma点来近似非线性观测函数和状态估计的高斯分布，从而提供更准确和稳定的估计结果，适用于高度非线性的系统。

综上所述，非线性观测函数是在卡尔曼滤波中处理非线性系统的重要技术。EKF和UKF是两种常用的方法，它们可以有效地处理非线性观测函数，提供更准确和稳定的状态估计。选择合适的方法取决于具体应用场景和系统的非线性度。