算法-UKF中Sigma点生成

cpp 复制代码

void UKF::MakeSigmaPoints() {

    Eigen::VectorXd x_aug_ = Eigen::VectorXd(n_x_);
    x_aug_.head(n_x_) = x_;

    Eigen::MatrixXd P_aug = Eigen::MatrixXd::Zero(n_x_, n_x_);
    // 转成正定矩阵
    P_aug = pdefinite_svd(P_);
    // LLT分解
    Eigen::MatrixXd L = P_aug.llt().matrixL();
    sigma_points = Eigen::MatrixXd(n_x_, 2 * n_x_ + 1);

    sigma_points.col(0) = x_aug_;
    const double c = std::sqrt(lamda_ + n_x_);

    for (int i = 0; i < n_x_; ++i) {
        sigma_points.col(i + 1) = x_aug_ + c * L.col(i);
        sigma_points.col(i + n_x_ + 1) = x_aug_ - c * L.col(i);
    }
}

代码中Sigma点生成解释如下：

在UKF（Unscented Kalman Filter，无迹卡尔曼滤波）中，sigma点是一组精心选择的采样点，用于近似非线性函数的传播和观测模型。这些sigma点基于系统的状态向量均值和协方差矩阵生成，旨在捕捉状态向量的分布特性。以下是关于UKF中sigma点的详细解释：

一、sigma点的生成

复制代码

确定状态向量和协方差矩阵：
    状态向量通常包含系统的状态变量。
    协方差矩阵描述了状态变量之间的关系，即各状态变量之间的协方差。
选择sigma点的数量：
    通常，sigma点的数量是状态向量维度的两倍加一，即如果状态向量的维度为n，则sigma点的数量为2n+1。
生成sigma点：
    根据状态向量的均值和协方差矩阵，通过特定的算法（如Julier-UT权重或Merwe-UT权重）生成sigma点。这些sigma点围绕状态向量的均值分布，能够较好地反映状态向量的概率分布特性。

二、sigma点的作用

复制代码

近似非线性函数的传播：
    在UKF的预测步骤中，将生成的sigma点通过系统的非线性状态方程传播到下一个时刻，得到预测的sigma点。这一步骤用于近似非线性函数在状态空间中的传播。
计算预测状态向量和协方差矩阵：
    通过对传播后的sigma点进行加权平均，计算预测状态向量和预测协方差矩阵。这些预测值反映了系统状态在下一时刻的估计值及其不确定性。
近似非线性观测模型：
    在UKF的更新步骤中，将预测的sigma点通过系统的非线性观测方程映射到观测空间，得到预测的观测值sigma点。这一步骤用于近似非线性观测模型在观测空间中的表现。
计算卡尔曼增益和更新状态向量：
    通过预测的观测值sigma点计算预测的观测向量和协方差矩阵，进而计算卡尔曼增益。最后，利用卡尔曼增益和实际的观测值对预测的状态向量进行修正，得到更新后的状态向量和协方差矩阵。

三、sigma点的优势

复制代码

处理非线性系统：
    UKF通过sigma点近似非线性函数的传播和观测模型，能够有效处理非线性系统。
提高估计精度：
    相比于EKF（Extended Kalman Filter，扩展卡尔曼滤波）通过泰勒展开将非线性系统线性化，UKF不需要忽略高阶项，因此通常能够获得更高的估计精度。
实现简单：
    UKF在实现上比EKF更为简单，因为它不需要计算复杂的雅可比矩阵。

综上所述，sigma点在UKF中扮演着至关重要的角色，它们通过近似非线性函数的传播和观测模型，实现了对非线性系统的有效滤波和状态估计。