• <bdo id="e8w02"><label id="e8w02"></label></bdo>
    写帖子

    最近搜索

      暂无相关数据

    注册 登录

    无人驾驶汽车系统入门(三)——无损卡尔曼滤波,目标追踪,C++

    说两句

    圈子: 2018-07-02 10:20:25

    无人驾驶汽车系统入门(三)——无损卡尔曼滤波,目标追踪,C++

    通过上一篇文章,我们已经知道KF不适用于非线性系统,为了处理非线性系统,我们通过一阶泰勒展式来近似(用线?#38498;?#25968;近似),这个方案直接的结果就是,我们对于具体的问题都要求解对应的一阶偏导(雅可比矩阵),求解雅可比矩阵本身就是费时的计算,而且我们上一篇还使用了Python而非C++,而且我们图省事还用了一个叫做numdifftools的库来实现雅可比矩阵的求解而不是自?#22411;?#23548;,这一切都造成了我们上一次博客的代码执行效率奇低!显然现实应用里面我们的无人车是不能接受这种延迟的,我相信很多同学也和我一样讨厌求解雅可比矩阵,那么,就让我们来学习一种相对简单的状态估计算法——UKF。

    UKF使用的是统计线性化技术,我们把这种线性化的方法叫做无损变换(unscented transformation)这一技术主要通过n个在先验分布中采集的点(我们把它们叫sigma points)的线性回归来线性化随机变量的非线?#38498;?#25968;,由于我们考虑的是随机变量的扩展,所以这种线性化要比泰勒级数线性化(EKF所使用的策略)更准确。和EKF一样,UKF也主要分为预测和更新

    运动模型

    本篇我们继续使用CTRV运动模型,不了解该模型的同学可以去看上一篇博客,CTRV模型的具体形式如下: 

    在EKF中,我们将直线加速度和偏航角加速度的影响当作我们的处理噪声,并且假设他们是满足均值为 

    00,方差为 σaσa 和 σω˙σω˙,在这里,我们将噪声的影响直接考虑到我们的状态转移函数中来。至于函数中的不确定项 μaμa 和 μω˙μω˙ 我们后面再分析。

    非线?#28304;?#29702;/测量模型

    我们知道我们在应用KF是面临的主要问题就是非线?#28304;?#29702;模型(?#28909;?#35828;CTRV)和非线性测量模型(RADAR测量)的处理。我们从概?#21490;?#24067;的角度来描述这个问题:

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

    UKF的基本思路就是: 近似非线?#38498;?#25968;的概?#21490;?#24067;要比近似非线?#38498;?#25968;本身要容易!

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

    无损变换

    计算一堆点(术语叫做sigma point),通过一定的手段产生的这些sigma点能够代表当前的分布,?#32531;?#23558;这些点通过非线?#38498;?#25968;(处理模型)变换成一些新的点,?#32531;?#22522;于这些新的sigma点计算出一个高斯分布(带有权重地计算出来)如图:

    预测

    由一个高斯分布产生sigma point

    通常,假定状态的个数为 nn ,我们会产生 2n+12n+1 个sigma点,其中第一个就是我们当前状态的均值 μμ ,sigma点集的均值的计算公式为:

    其中的 λλ 是一个超参数,根据公式,λλ 越大, sigma点就越远离状态的均值,λλ ?#21483;。?sigma点就越靠近状态的均值。需要注意的是,在我们的CTRV模型中,状态数量 nn 除了要包含5个状态以外,还要包含处理噪声 μaμa 和 μω˙μω˙,因为这些处理噪声对模型也有着非线性的影响。在增加了处理噪声的影响?#38498;螅?#25105;们的不确定性矩阵 PP 就变成了:

    其中,PP′ 就是我们原来的不确定性矩阵(在CTRV模型中就是一个 5×55×5 的矩阵),Q是处理噪声的协方差矩阵,在CTRV模型中考虑到直线加速度核Q的形式为:

    其中的 σ2aσa2 , σ2ω˙σω˙2 和我们上一篇博客讲的一样。以上公式中还存在一个问题,那就是矩阵开平方根怎么计算的问题,同产情况下,我们求得是:


    求解上式中的 AA 是一个相对复杂的过程,但是如果 PP 是对角矩阵的话,那么这个求解就会简化,在我们的?#36947;?#20013;,P表示对估计状态的不确定性(协方差矩阵),我们会发现 PP 基本上是对角矩阵(状态量之间的相关性几乎为0), 所以我们可以首先对 PP 做 Cholesky分解(Cholesky decomposition) ,?#32531;?#20998;解得到的矩阵的下三角矩阵就是我们要求的 AA ,在这里我们就不详细讨论了,在我们后面的?#23548;?#20363;子中,我们直接调用库中相应的方法即可(注意:本次的?#36947;?#25105;们换C++来实现,相比于Python,C++更加贴近我们?#23548;?#30340;开发):

    预测sigma point

    http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.136.6539&rep=rep1&type=pdf

    https://www.cse.sc.edu/~terejanu/files/tutorialUKF.pdf

    http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/pdf/slam06-ukf-4.pdf

    UKF?#36947;?/span>

    我们继续基于上一篇文章的数据来做状态估计的?#36947;?#19981;过,为了更加接近?#23548;适导?#30340;应用,我们本节采用C++实现。由于本节使用的C++代码量相对较大,而且为多文件的项目,代码就不再一一贴出,所?#20889;?#30721;我都已经上传至如下地址:

    http://download.csdn.net/download/adamshan/10041096

    运行-效果

    首先解压,编译:

    mkdir buildcd build
    cmake ..
    make
    • 1
    • 2
    • 3
    • 4

    运行:

    ./ukf ../data/data_synthetic.txt ../data/output.txt
    • 1

    得到计算的结果:

    Accuracy - RMSE:0.07234080.0821208
     0.342265
      0.23017
    • 1
    • 2
    • 3
    • 4
    • 5

    我们发现,这个UKF要比我们上一篇博客写的EKF的效率要高得多得多。

    我们执行可视化的python脚本看一下效果:

    cd ../data
    python plot.py
    • 1
    • 2

    得到如下结果:

    放大一点:

    UKF在我们这个问题中的执行效率和估计精度都高于我们上一篇文章中实现的EKF,下面就让我们看一下核心代码。

    核心代码

    首先是预测,预测主要包含三部分,?#30452;?#26159;:

    • 产生sigma点集
    • 基于CTRV模型预测sigma点集
    • 计算新的均值核方差

    产生sigma点集

    void UKF::AugmentedSigmaPoints(MatrixXd *Xsig_out) {    //create augmented mean vector
        VectorXd x_aug = VectorXd(7);    //create augmented state covariance
        MatrixXd P_aug = MatrixXd(7, 7);    //create sigma point matrix
        MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);    //create augmented mean state
        //create augmented covariance matrix
        //create square root matrix
        //create augmented sigma points
        x_aug.head(5) = x_;
        x_aug(5) = 0;
        x_aug(6) = 0;
    
        P_aug.fill(0.0);
        P_aug.topLeftCorner(5, 5) = P_;
        P_aug(5,5) = std_a_*std_a_;
        P_aug(6,6) = std_yawdd_*std_yawdd_;
    
        MatrixXd A = P_aug.llt().matrixL();    //create augmented sigma points
        Xsig_aug.col(0)  = x_aug;    for (int i = 0; i< n_aug_; i++)
        {
            Xsig_aug.col(i+1)       = x_aug + sqrt(lambda_+n_aug_) * A.col(i);
            Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt(lambda_+n_aug_) * A.col(i);
        }    //write result
        *Xsig_out = Xsig_aug;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37

    基于CTRV模型预测sigma点集

    void UKF::SigmaPointPrediction(MatrixXd &Xsig_aug, double delta_t) {
    
        for(int i =0; i < (2 * n_aug_ + 1); i++){
            VectorXd input_x = Xsig_aug.col(i);
            float px = input_x[0];
            float py = input_x[1];
            float v = input_x[2];
            float psi = input_x[3];
            float psi_dot = input_x[4];
            float mu_a = input_x[5];
            float mu_psi_dot_dot = input_x[6];
    
            VectorXd term2 = VectorXd(5);
            VectorXd term3 = VectorXd(5);
    
            VectorXd result = VectorXd(5);
            if(psi_dot < 0.001){
                term2 << v * cos(psi) * delta_t, v * sin(psi) * delta_t, 0, psi_dot * delta_t, 0;
                term3 << 0.5 * delta_t*delta_t * cos(psi) * mu_a,                    0.5 * delta_t*delta_t * sin(psi) * mu_a,
                        delta_t * mu_a,                    0.5 * delta_t*delta_t * mu_psi_dot_dot,
                        delta_t * mu_psi_dot_dot;
                result = Xsig_aug.col(i).head(5) + term2 + term3;
            } else{
                term2 << (v/psi_dot) * (sin(psi + psi_dot * delta_t) - sin(psi)),
                        (v/psi_dot) * (-cos(psi + psi_dot * delta_t) + cos(psi)),                    0,
                        psi_dot * delta_t,                    0;
    
                term3 << 0.5 * delta_t*delta_t * cos(psi) * mu_a,                    0.5 * delta_t*delta_t * sin(psi) * mu_a,
                        delta_t * mu_a,                    0.5 * delta_t*delta_t * mu_psi_dot_dot,
                        delta_t * mu_psi_dot_dot;
                result = Xsig_aug.col(i).head(5) + term2 + term3;
            }
    
            Xsig_pred_.col(i) = result;
        }
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42

    计算新的均值核方差

    void UKF::PredictMeanAndCovariance() {    x_.fill(0.0);    for(int i=0; i<2*n_aug_+1; i++){        x_ = x_+ weights_[i] * Xsig_pred_.col(i);
        }
    
        P_.fill(0.0);    for(int i=0;  i<2*n_aug_+1; i++){
            VectorXd x_diff = Xsig_pred_.col(i) - x_;        while (x_diff[3]> M_PI)
                x_diff[3] -= 2.*M_PI;        while (x_diff[3] <-M_PI)
                x_diff[3]+=2.*M_PI;
            P_ = P_ + weights_[i] * x_diff * x_diff.transpose();
        }
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16

    测量更新主要分为:

    • 预测LIDAR测量
    • 预测RADAR测量
    • 更新状态

    预测LIDAR测量

    void UKF::PredictLaserMeasurement(VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) {    for(int i=0; i < 2*n_aug_+1; i++){        float px = Xsig_pred_.col(i)[0];        float py = Xsig_pred_.col(i)[1];
    
            VectorXd temp = VectorXd(n_z);
            temp << px, py;
            Zsig.col(i) = temp;
        }
    
        z_pred.fill(0.0);    for(int i=0; i < 2*n_aug_+1; i++){
            z_pred = z_pred + weights_[i] * Zsig.col(i);
        }
    
        S.fill(0.0);    for(int i=0; i < 2*n_aug_+1; i++){        //residual
            VectorXd z_diff = Zsig.col(i) - z_pred;
    
            S = S + weights_[i] * z_diff * z_diff.transpose();
        }
        S = S + R_laser_;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24

    预测RADAR测量

    void UKF::PredictRadarMeasurement(VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) {    for(int i=0; i < 2*n_aug_+1; i++){        float px = Xsig_pred_.col(i)[0];        float py = Xsig_pred_.col(i)[1];        float v = Xsig_pred_.col(i)[2];        float psi = Xsig_pred_.col(i)[3];        float psi_dot = Xsig_pred_.col(i)[4];        float temp = px * px + py * py;        float rho = sqrt(temp);        float phi = atan2(py, px);        float rho_dot;        if(fabs(rho) < 0.0001){
                rho_dot = 0;
            } else{
                rho_dot =(px * cos(psi) * v + py * sin(psi) * v)/rho;
            }
    
            VectorXd temp1 = VectorXd(3);
            temp1 << rho, phi, rho_dot;
            Zsig.col(i) = temp1;
        }
    
        z_pred.fill(0.0);    for(int i=0; i < 2*n_aug_+1; i++){
            z_pred = z_pred + weights_[i] * Zsig.col(i);
        }
    
        S.fill(0.0);    for(int i=0; i < 2*n_aug_+1; i++){        //residual
            VectorXd z_diff = Zsig.col(i) - z_pred;        //angle normalization
            while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;        while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
            S = S + weights_[i] * (Zsig.col(i) - z_pred) * (Zsig.col(i) - z_pred).transpose();
        }
        S = S + R_radar_;
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40

    更新状态

    void UKF::UpdateState(VectorXd &z, VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) {    //create matrix for cross correlation Tc
        MatrixXd Tc = MatrixXd(n_x_, n_z);    //calculate cross correlation matrix
        //calculate Kalman gain K;
        //update state mean and covariance matrix
    
        Tc.fill(0.0);    for(int i=0; i < 2*n_aug_+1; i++){
            VectorXd x_diff = Xsig_pred_.col(i) - x_;        while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;        while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;        //residual
            VectorXd z_diff = Zsig.col(i) - z_pred;        if(n_z == 3){            //angle normalization
                while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;            while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
            }
            Tc = Tc + weights_[i] * x_diff * z_diff.transpose();
        }
    
        MatrixXd K = MatrixXd(5, 3);
        K = Tc * S.inverse();
    
        VectorXd y = z - z_pred;    //angle normalization
        if(n_z == 3){        while (y(1)> M_PI) y(1)-=2.*M_PI;        while (y(1)<-M_PI) y(1)+=2.*M_PI;
        }
        x_ = x_ + K * y;
        P_ = P_ - K * S * K.transpose();
    }
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38

    以上就是我们的UKF的核心算法实现了,可能光看这些核心代码还是?#35805;?#27861;理解,所以?#34892;?#36259;的童鞋就去下载来再详细研究把~

    上一篇文章我买了个关子,没有贴出把结果可视化的代码,本次代码已经一本包含在项目里(具体见/data/plot.py)

    本文摘自csdn博客http://blog.csdn.net/adamshan/article/details/78359048


    显示全部

    0个评论

      数据加载中...

    欢迎来到汽车学堂!

    请及时完善个人信息,增加就业推荐机会

    欢迎来到汽车学堂!

    请及时完善个人信息,为您提供更好的服务

    下线通知

    您的帐号在其它地方登录,您已被迫下线,如果不 是您本人操作,请及时修改密码。

    浙江20选5玩法