关于无迹卡尔曼滤波（UKF）

最新推荐文章于 2024-07-23 17:36:37 发布

棒球部前部长

最新推荐文章于 2024-07-23 17:36:37 发布

阅读量452

点赞数 3

文章标签：算法

本文链接：https://blog.csdn.net/bangqiubu/article/details/140069317

版权

UKF（无迹卡尔曼滤波）与传统的KF（卡尔曼滤波）、EKF（扩展卡尔曼滤波）和ESKF（误差状态卡尔曼滤波）有显著的不同。虽然它们在逻辑上都是通过预测和更新过程来进行状态估计，但实现形式却有很大差异。

KF、EKF和ESKF都是基于线性化的状态转移方程和观测方程。其中，EKF和ESKF通过将非线性的状态转移方程和观测方程进行线性化来实现预测和更新过程。然而，UKF则不会将非线性方程线性化，而是通过无迹变换（Unscented Transform, UT）来推测非线性过程的后验概率分布。

学术上来说，无迹卡尔曼滤波基于无迹变换。无迹变换研究的是如何通过确定的采样点来捕获经过非线性变换后的高斯随机变量的后验分布。在得到相应的统计特性后，再结合标准卡尔曼滤波框架，就形成了无迹卡尔曼滤波。

通俗地讲，假设有一段从起点到终点的路程，这个过程可以离散化为很多分布为非线性的点。EKF会尽可能将这些非线性的点集拟合成一个线性化的点集，而UKF则会尽可能找到并利用这些非线性的点集来进行状态估计。

UKF算法框架：

第一步，初始化状态变量和协方差，可以自己设定也可以由观测得到；设定无迹变换的参数 $\alpha ,\beta ,\kappa ,\lambda$ ;根据无迹变换参数计算各sigma的权重：

$W_m^{(i)} = \begin{cases} \frac{\lambda}{n+\lambda} & \text{if } i = 0, \\ \frac{1}{2(n+\lambda)} & \text{if } i = 1, \ldots, 2n. \end{cases}\\W_c^{(i)} = \begin{cases} \frac{\lambda + 1 - \alpha^2 + \beta}{n+\lambda} & \text{if } i = 0, \\ \frac{1}{2(n+\lambda)} & \text{if } i = 1, \ldots, 2n. \end{cases}$

其中， $W_m^{(i)}$ 是用于计算状态变量的权重， $W_c^{(i)}$ 是用于计算协方差的权重。

第二步，对 $k-1$ 时刻的状态变量 $X_{k-1}$ 的后验值（状态变量和协方差）进行sigma采样：

$X_{k-1}^{+(i)} = \begin{cases} \mu^+_{X_{k-1}} & \text{if } i = 0, \\ \mu^+_{X_{k-1}} + \gamma \left(\sqrt{P^+_{X_{k-1}}}\right)^{(i-1)} & \text{if } i = 1, \ldots, n, \\ \mu^+_{X_{k-1}} - \gamma \left(\sqrt{P^+_{X_{k-1}}}\right)^{(i-n-1)} & \text{if } i = n+1, \ldots, 2n. \end{cases}$

其中， $\gamma = \sqrt{(n + \lambda)}$ ， $\mu^+_{X_{k-1}},P^+_{X_{k-1}}$ 代表 $k-1$ 时刻后验值。

第三步，将每个sigma点代入非线性方程，得到 $k$ 时刻每个sigma点的预测值：

$X_k^{-*(i)} = f(X_{k-1}^{+(i)}, u_k), \quad i = 0, 1, 2, \ldots, 2n$

第四步，加权计算 $k$ 时刻状态变量 $X_{k}$ 的先验值（状态变量和协方差），也可以说是一个全局预测：

$\mu^-_{X_k} = \sum_{i=0}^{2n} W_m^{(i)} X_k^{-*(i)} \\P^-_{X_k} = \sum_{i=0}^{2n} W_c^{(i)} \left( X_k^{-*(i)} - \mu^-_{X_k} \right) \left( X_k^{-*(i)} - \mu^-_{X_k} \right)^T + Q$

第五步，对 $X_{k}$ 的先验值进行sigma采样，更新sigma点：

$X_k^{-(i)} = \begin{cases} \mu^-_{X_k} & \text{if } i = 0, \\ \mu^-_{X_k} + \gamma \left(\sqrt{P^-_{X_k}}\right)^{(i-1)} & \text{if } i = 1, \ldots, n, \\ \mu^-_{X_k} - \gamma \left(\sqrt{P^-_{X_k}}\right)^{(i-n-1)} & \text{if } i = n+1, \ldots, 2n. \end{cases}$

第六步，将每个sigma点的先验值代入观测方程得到每个sigma点对应的观测预测值：

$Z_k^{(i)} = h(X_k^{-{(i)}}), \quad i = 0, 1, 2, \ldots, 2n$

第七步，加权计算 $k$ 时刻 $z_{k}$ 的概率分布（观测的全局预测）：

$\mu_{Z_k} = \sum_{i=0}^{2n} W_m^{(i)} Z_k^{(i)} \\P_{Z_k} = \sum_{i=0}^{2n} W_c^{(i)} \left( Z_k^{(i)} - \mu_{Z_k} \right) \left( Z_k^{(i)} - \mu_{Z_k} \right)^T + R$

第八步，计算状态变量和观测量的互协方差：

$P_{X_kZ_k} = \sum_{i=0}^{2n} W_c^{(i)} \left( X_k^{-(i)} - \mu_{X_k}^{-} \right) \left( Z_k^{(i)} - \mu_{Z_k} \right)^T$

第九步，计算卡尔曼增益：

$K_k = P_{X_kZ_k} P_{Z_k}^{-1}$

第十步，计算 $k$ 时刻状态变量的后验概率分布（ $k$ 时刻状态变量和协方差）：

$\mu^+_{X_k} = \mu^-_{X_k} + K_k (z_k - \mu_{Z_k}) \\P^+_{X_k} = P^-_{X_k} - K_k P_{Z_k} K_k^T$

#ifndef UNSCENTED_KALMAN_FILTER_
#define UNSCENTED_KALMAN_FILTER_

#include "Eigen/Dense"

class UnscentedKalmanFilter
{
private:
    bool is_inited;          // 初始化标志
    bool just_begin_filt;    // 刚开始过滤数据的标志
    uint64_t timestamp_last; // 上一帧的时间戳: 微秒
    double dt;               // 时间差: 秒

    int n_x;     // 状态向量X的维度
    int n_x_aug; // 扩增状态向量X的维度
    int n_z;     // 测量向量Z的维度

    double std_a;         // 纵向加速度过程噪声的标准差: 米/秒^2
    double std_omega_dot; // 偏航加速度过程噪声的标准差: 弧度/秒^2
    Eigen::MatrixXd Q;    // 过程噪声协方差矩阵

    double std_rho;     // 测量噪声距离的标准差: 米
    double std_phi;     // 测量噪声方位角的标准差: 弧度
    double std_rho_dot; // 测量噪声距离变化率的标准差: 米/秒
    Eigen::MatrixXd R;  // 测量噪声协方差矩阵

    double kappa;            // 无迹变换参数κ
    Eigen::VectorXd weights; // sigma点的权重向量

    Eigen::VectorXd X;   // 状态向量
    Eigen::MatrixXd P_x; // 状态协方差矩阵

    Eigen::VectorXd X_aug;         // 扩增状态向量
    Eigen::MatrixXd P_x_aug;       // 扩增状态协方巧矩阵
    Eigen::MatrixXd Sigmas_x_aug;  // 扩增的X(k-1)后验概率分布的sigma点
    Eigen::MatrixXd Sigmas_x_pred; // X(k)先验概率分布的sigma点

    Eigen::MatrixXd Sigmas_z_pred; // Z(k)概率分布的sigma点

    Eigen::VectorXd Z;   // 测量向量
    Eigen::MatrixXd P_z; // 测量协方巧矩阵

    Eigen::MatrixXd P_xz; // X与Z的交叉协方巧矩阵
    Eigen::MatrixXd K;    // 卡尔曼增益系数: K = P_xz * P_z.inverse()

public:
    Eigen::VectorXd Z_meas; // 测量值向量
    uint64_t timestamp_now; // 当前帧的时间戳: 微秒

public:
    /**
     * @brief 构造一个新的无迹卡尔曼滤波器对象
     *
     */
    UnscentedKalmanFilter();

    /**
     * @brief 销毁无迹卡尔曼滤波器对象
     *
     */
    ~UnscentedKalmanFilter();

    /**
     * @brief 返回ukf实例的初始化状态。
     *
     * @return bool
     */
    inline bool &IsInited() { return is_inited; }

    /**
     * @brief 重置ukf实例。
     *
     */
    inline void Reset() { is_inited = false; }

    /**
     * @brief 规范化输入角度到[-PI, PI]。
     *
     * @param angle
     */
    inline void NormalizeAngle(double &angle)
    {
        while (angle > M_PI)
            angle -= 2.0 * M_PI;
        while (angle < -M_PI)
            angle += 2.0 * M_PI;
    }

    /**
     * @brief 更新测量值向量和时间戳。
     *
     */
    inline void RecvRawData()
    {
        // Z_meas[0] = rho_new;
        // Z_meas[1] = phi_new;
        // Z_meas[2] = rho_dot_new;
        // timestamp_now = timestamp_new;

        dt = (timestamp_now - timestamp_last) / 1E6;
        timestamp_last = timestamp_now;
    }

    /**
     * @brief 初始化步骤。
     *
     */
    void Init();

    /**
     * @brief 扩增X(k-1)的后验概率分布。
     *
     */
    void AugmentLastPosteriorPDF();

    /**
     * @brief 抽样X(k-1)的扩增后验概率分布。
     *
     */
    void SigmaSample();

    /**
     * @brief 预测步骤。
     *
     */
    void Predict();

    /**
     * @brief 更新步骤。
     *
     */
    void Update();
};

#endif

#include "math.h"
#include "UnscentedKalmanFilter.h"

/**
 * @brief 构建一个新的无迹卡尔曼滤波器对象
 *
 */
UnscentedKalmanFilter::UnscentedKalmanFilter()
    : is_inited(false),
      just_begin_filt(false),
      timestamp_last(0),
      dt(0.0),
      n_x(5),
      n_x_aug(7),
      n_z(3),
      std_a(1.5),         // 需根据ukf的性能调整
      std_omega_dot(0.8), // 需根据ukf的性能调整
      Q(2, 2),
      std_rho(0.3),
      std_phi(0.03),
      std_rho_dot(0.3),
      R(n_z, n_z),
      kappa(0.0),
      weights(2 * n_x_aug + 1),
      X(n_x),
      P_x(n_x, n_x),
      X_aug(n_x_aug),
      P_x_aug(n_x_aug, n_x_aug),
      Sigmas_x_aug(n_x_aug, 2 * n_x_aug + 1),
      Sigmas_x_pred(n_x, 2 * n_x_aug + 1),
      Sigmas_z_pred(n_z, 2 * n_x_aug + 1),
      Z(n_z),
      P_z(n_z, n_z),
      P_xz(n_x, n_z),
      K(n_x, n_z),
      Z_meas(n_z),
      timestamp_now(0)
{
}

/**
 * @brief 销毁无迹卡尔曼滤波器对象
 *
 */
UnscentedKalmanFilter::~UnscentedKalmanFilter() {}

/**
 * @brief 初始化无迹卡尔曼滤波器。
 *
 */
void UnscentedKalmanFilter::Init()
{
    if (!IsInited())
    {
        double rho = Z_meas(0);
        double phi = Z_meas(1);
        double rho_dot = Z_meas(2);

        double px = rho * cos(phi);
        double py = rho * sin(phi);

        // 初始状态向量X(2)的设定：0，一个小值，还是初始测量值rho_dot?
        X << px, py, 0.0, 0.0, 0.0;

        P_x << 1.0, 0.0, 0.0, 0.0, 0.0,
               0.0, 1.0, 0.0, 0.0, 0.0,
               0.0, 0.0, 1.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 1.0, 0.0,
               0.0, 0.0, 0.0, 0.0, 1.0;

        Q << std_a * std_a, 0.0,
             0.0, std_omega_dot * std_omega_dot;

        R << std_rho * std_rho, 0.0, 0.0,
             0.0, std_phi * std_phi, 0.0,
             0.0, 0.0, std_rho_dot * std_rho_dot;

        kappa = 3 - n_x_aug;

        weights.fill(1 / (2 * (n_x_aug + kappa)));
        weights(0) = kappa / (n_x_aug + kappa);

        is_inited = true;
        just_begin_filt = true;
    }
    else
    {
        just_begin_filt = false;
    }
}

/**
 * @brief 扩增X(k-1)的后验概率分布。
 *
 */
void UnscentedKalmanFilter::AugmentLastPosteriorPDF()
{
    if (just_begin_filt)
        return;

    // 扩增均值
    X_aug.head(5) = X;
    X_aug(5) = 0.0;
    X_aug(6) = 0.0;

    // 扩增协方差
    P_x_aug.fill(0.0);
    P_x_aug.topLeftCorner(5, 5) = P_x;
    P_x_aug(5, 5) = Q(0, 0);
    P_x_aug(6, 6) = Q(1, 1);
}

/**
 * @brief 抽样X(k-1)的扩增后验概率分布。
 *
 */
void UnscentedKalmanFilter::SigmaSample()
{
    if (just_begin_filt)
        return;

    double gamma = std::sqrt(n_x_aug + kappa);
    Eigen::MatrixXd chol_root = P_x_aug.llt().matrixL();

    Sigmas_x_aug.col(0) = X_aug;
    for (int i = 1; i < 2 * n_x_aug + 1; ++i)
    {
        if (i < n_x_aug + 1)
            Sigmas_x_aug.col(i) = X_aug + gamma * chol_root.col(i - 1);
        else
            Sigmas_x_aug.col(i) = X_aug - gamma * chol_root.col(i - n_x_aug - 1);
    }
}

/**
 * @brief 预测步骤。
 *
 */
void UnscentedKalmanFilter::Predict()
{
    if (just_begin_filt)
        return;

    // 预测X(k)先验概率分布的sigma点
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
    {
        double px = Sigmas_x_aug(0, i);
        double py = Sigmas_x_aug(1, i);
        double v = Sigmas_x_aug(2, i);
        double psi = Sigmas_x_aug(3, i);
        double omega = Sigmas_x_aug(4, i);
        double a = Sigmas_x_aug(5, i);
        double omega_dot = Sigmas_x_aug(6, i);

        Eigen::VectorXd state_trans_item_motion(n_x);
        Eigen::VectorXd state_trans_item_noise(n_x);

        state_trans_item_noise << 0.5 * a * cos(psi) * dt * dt,
            0.5 * a * sin(psi) * dt * dt,
            a * dt,
            0.5 * a * omega_dot * dt * dt,
            omega_dot * dt;

        if (std::fabs(omega_dot) > 0.001) // 使用CTRV模型
        {
            state_trans_item_motion << v / omega * (sin(psi + omega * dt) - sin(psi)),
                v / omega * (-cos(psi + omega * dt) + cos(psi)),
                0.0,
                omega * dt,
                0;
        }
        else // 使用近似CV模型
        {
            state_trans_item_motion << v * cos(psi) * dt,
                v * sin(psi) * dt,
                0.0,
                omega * dt,
                0;
        }

        Sigmas_x_pred.col(i) = Sigmas_x_aug.head(5) + state_trans_item_motion + state_trans_item_noise;
    }

    // 计算X(k)先验PDF的近似均值
    X.fill(0.0);
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
        X += weights(i) * Sigmas_x_pred.col(i);

    // 计算X(k)先验PDF的近似协方差
    P_x.fill(0.0);
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
    {
        Eigen::VectorXd x_diff = Sigmas_x_pred.col(i) - X;
        NormalizeAngle(x_diff(3));

        P_x += weights(i) * x_diff * x_diff.transpose();
    }
}

/**
 * @brief 更新步骤。
 *
 */
void UnscentedKalmanFilter::Update()
{
    if (just_begin_filt)
        return;

    // 预测Z(k)概率分布的sigma点
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
    {
        double px = Sigmas_x_pred(0, i);
        double py = Sigmas_x_pred(1, i);
        double v = Sigmas_x_pred(2, i);
        double psi = Sigmas_x_pred(3, i);

        Sigmas_z_pred(0, i) = std::sqrt(px * px + py * py);
        Sigmas_z_pred(1, i) = atan2(py, px);
        Sigmas_z_pred(2, i) = (v * cos(psi) * px + v * sin(psi) * py) / Sigmas_z_pred(0, i);
    }

    // 计算Z(k)PDF的近似均值
    Z.fill(0.0);
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
        Z += weights(i) * Sigmas_z_pred.col(i);

    // 计算Z(k)PDF的近似协方差
    P_z.fill(0.0);
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
    {
        Eigen::VectorXd z_diff = Sigmas_z_pred.col(i) - Z;
        NormalizeAngle(z_diff(1));

        P_z += weights(i) * z_diff * z_diff.transpose();
    }
    P_z += R;

    // 计算X(k)与Z(k)的交叉协方差
    P_xz.fill(0.0);
    for (int i = 0; i < 2 * n_x_aug + 1; ++i)
    {
        Eigen::VectorXd x_diff = Sigmas_x_pred.col(i) - X;
        NormalizeAngle(x_diff(3));

        Eigen::VectorXd z_diff = Sigmas_z_pred.col(i) - Z;
        NormalizeAngle(z_diff(1));

        P_xz += weights(i) * x_diff * z_diff.transpose();
    }

    // 计算卡尔曼增益
    K = P_xz * P_z.inverse();

    // 计算X(k)后验PDF的均值
    Eigen::VectorXd z_diff = Z_meas - Z;
    NormalizeAngle(z_diff(1));
    X += K * z_diff;

    // 计算X(k)后验PDF的协方差
    P_x -= K * P_z * K.inverse();
}

#include "UnscentedKalmanFilter.h"

int main()
{
    UnscentedKalmanFilter ukf;

    while (true)
    {
        ukf.RecvRawData();

        ukf.Init();
        ukf.AugmentLastPosteriorPDF();
        ukf.SigmaSample();
        ukf.Predict();
        ukf.Update();
    }

    return 0;
}

主要参考： blog.shipengx.com

棒球部前部长

关注

3
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
关于无迹卡尔曼滤波（UKF）

其中，EKF和ESKF通过将非线性的状态转移方程和观测方程进行线性化来实现预测和更新过程。然而，UKF则不会将非线性方程线性化，而是通过无迹变换（Unscented Transform, UT）来推测非线性过程的后验概率分布。UKF（无迹卡尔曼滤波）与传统的KF（卡尔曼滤波）、EKF（扩展卡尔曼滤波）和ESKF（误差状态卡尔曼滤波）有显著的不同。无迹变换研究的是如何通过确定的采样点来捕获经过非线性变换后的高斯随机变量的后验分布。在得到相应的统计特性后，再结合标准卡尔曼滤波框架，就形成了无迹卡尔曼滤波。
复制链接

扫一扫