zoukankan      html  css  js  c++  java
  • 无迹卡尔曼滤波器详解

    @

    一、 非线性处理/测量模型

    我们知道KF是面临的主要问题就是非线性处理模型(比如说CTRV)和非线性测量模型(RADAR测量)的处理。我们从概率分布的角度来描述这个问题:

    对于我们想要估计的状态,在(k)时刻满足均值为(x_k),方差为(P_k) 这样的一个高斯分布,这个是我们在k时刻的 后验(Posterior)(如果我们把整个卡尔曼滤波的过程迭代的来考虑的话),现在我们以这个后验为出发点,结合一定的先验知识(比如说CTRV运动模型)去估计我们在 k+1时刻的状态的均值和方差,这个过程就是卡尔曼滤波的预测,如果变换是线性的,那么预测的结果仍然是高斯分布,但是现实是我们的处理和测量模型都是非线性的,结果就是一个不规则分布,KF能够使用的前提就是所处理的状态是满足高斯分布的,为了解决这个问题,EKF是寻找一个线性函数来近似这个非线性函数,而UKF就是去找一个与真实分布近似的高斯分布

    UKF的基本思路就是: 近似非线性函数的概率分布要比近似非线性函数本身要容易!

    那么如何去找一个与真实分布近似的高斯分布呢?——找一个与真实分布有着相同的均值和协方差的高斯分布。那么如何去找这样的均值和方差呢?——无损变换。

    UT变换是用固定数量的参数去近似一个高斯分布,其实现原理为:在原先分布中按某一规则取一些点,使这些点的均值和协方差与原状态分布的均值和协方差相等;将这些点代入非线性函数中,相应得到非线性函数值点集,通过这些点集可求取变换的均值和协方差。对任何一种非线性系统,当高斯型状态微量经由非线性系统进行传递,进而利用这组采样点能获取精确到三阶矩的后验均值和协方差。

    UT变换的特点是对非线性函数的概率密度分布进行近似,而不是对非线性函数进行近似,即使系统模型复杂,也不增加算法实现的难度;而且所得到的非线性函数的统计量的准确性可以达到三阶;除此之外,它不需要计算雅可比矩阵,可以处理不可导非线性函数。

    二、无损(迹)变换(Unscented Transformation)

    通过一定的手段产生的这些sigma点能够代表当前的分布,然后将这些点通过非线性函数(系统模型)变换成一些新的点,然后基于这些新的sigma点计算出一个高斯分布(带有权重地计算出来)

    2.1 一个高斯分布产生sigma point

    UT变换基本原理如下:假设一个非线性系统(y=g(x)),其中(x)(n)维状态向量,并已知其平均值为(overline x),方差为(P_x),则可以经过UT变换构造(2n+1)个Sigma点,同时构造相应的权值,进而得到y的统计特性。

    构造sigma点集的计算公式为:
    在这里插入图片描述
    其中的 λ 是比例因子,根据公式,λ 越大, sigma点就越远离状态的均值,λ 越小, sigma点就越靠近状态的均值。

    [lambda = alpha(n^2+k)-n ]

    ((sqrt {(n+lambda)P_x})_i)表示矩阵方根(sqrt {(n+lambda)P_x})的第(i)列。

    可以使用Cholesky分解计算出矩阵(L(Σ = LL^T))

      //calculate square root of P
      MatrixXd A=P.llt().matrix();
    
    /**
       * @brief compute sigma points  生成sigma points
       * @param mean          mean
       * @param cov           covariance
       * @param sigma_points  calculated sigma points
       */
      void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
        const int n = mean.size(); // 状态x的维度
        assert(cov.rows() == n && cov.cols() == n);
    
        Eigen::LLT<MatrixXt> llt;
        llt.compute((n + lambda) * cov);
        MatrixXt l = llt.matrixL();
    
        sigma_points.row(0) = mean;
        for (int i = 0; i < n; i++) {
          sigma_points.row(1 + i * 2) = mean + l.col(i);
          sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
        }
      }
    

    (overline x)周围Sigma点的分布状态由(alpha)决定,调节(alpha)可以降低高阶项的影响,通常设为一个较小的正数,这里选取(alpha=0.001)(k)的取值没有具体设定限制,但至少应当保证矩阵({(n+lambda)P_x})为半正定矩阵,通常设置(k=0)

    2.2 sigma point的权重

    权重的选择应该满足下面的性质,假设y=g(x)
    在这里插入图片描述在这里插入图片描述
    因此权重w的公式如下:
    在这里插入图片描述
    如果x是高斯分布,(eta=2)是最佳的

    	// 均值的权重
        weights[0] = lambda / (N + lambda);
        for (int i = 1; i < 2 * N + 1; i++) {
          weights[i] = 1 / (2 * (N + lambda));
        }
    

    2.3 预测新的状态分布(predict过程)

    1. 使用系统方程更新所有sigma points的状态

    [hat sigma=f(sigma) ]

    // 遍历每天一个sigma points 更新他们的状态
    for (int i = 0; i < S; i++) 
    { 
      sigma_points.row(i) = system.f(sigma_points.row(i), control);
    }
    
    1. 加权新的sigma points 预测估计值和协方差

    [hat z=sum w_isigma_i ]

    [hat P=sum w_i(sigma_i-hat z)(sigma_i-hat z)^T+Q_{(过程噪声)} ]

    	const auto& Q = process_noise;
    
        // unscented transform
        VectorXt mean_pred(mean.size());
        MatrixXt cov_pred(cov.rows(), cov.cols());
    
        mean_pred.setZero();
        cov_pred.setZero();
        for (int i = 0; i < S; i++) {
          mean_pred += weights[i] * sigma_points.row(i);
        }
        for (int i = 0; i < S; i++) {
          VectorXt diff = sigma_points.row(i).transpose() - mean_pred;
          cov_pred += weights[i] * diff * diff.transpose();
        }
        cov_pred += Q;
    
        mean = mean_pred;
        cov = cov_pred;
    

    2.4 更新滤波器(measurement过程)

    用系统方程g(x),根系sigma point,

    参考:https://blog.csdn.net/qq_34461089/article/details/88989076#commentBox

  • 相关阅读:
    vue开发chrome扩展,数据通过storage对象获取
    Vue手动集成less预编译器
    Google Translate寻找之旅
    Javascript Range对象的学习
    Javascript Promises学习
    SublimeText 建立构建Node js系统
    We're sorry but demo3 doesn't work properly without JavaScript enabled. Please enable it to continue.
    npm安装包出现UNMET DEPENDENCY报错
    (转载)命令行说明中格式 尖括号 中括号的含义
    Linux重启网卡服务Failed to start LSB: Bring up/down networking.
  • 原文地址:https://www.cnblogs.com/long5683/p/14091520.html
Copyright © 2011-2022 走看看