• <dl id="as3yr"><ins id="as3yr"><thead id="as3yr"></thead></ins></dl>
  • <dl id="as3yr"></dl>
    <dl id="as3yr"><ins id="as3yr"></ins></dl>
  • <dl id="as3yr"></dl>
  • <progress id="as3yr"><tr id="as3yr"></tr></progress>
    <dl id="as3yr"></dl>
  • 寫帖子

    最近搜索

      暫無相關數據

    注冊 登錄

    無人駕駛汽車系統入門(三)——無損卡爾曼濾波,目標追蹤,C++

    說兩句

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

    無人駕駛汽車系統入門(三)——無損卡爾曼濾波,目標追蹤,C++

    通過上一篇文章,我們已經知道KF不適用于非線性系統,為了處理非線性系統,我們通過一階泰勒展式來近似(用線性函數近似),這個方案直接的結果就是,我們對于具體的問題都要求解對應的一階偏導(雅可比矩陣),求解雅可比矩陣本身就是費時的計算,而且我們上一篇還使用了Python而非C++,而且我們圖省事還用了一個叫做numdifftools的庫來實現雅可比矩陣的求解而不是自行推導,這一切都造成了我們上一次博客的代碼執行效率奇低!顯然現實應用里面我們的無人車是不能接受這種延遲的,我相信很多同學也和我一樣討厭求解雅可比矩陣,那么,就讓我們來學習一種相對簡單的狀態估計算法——UKF。

    UKF使用的是統計線性化技術,我們把這種線性化的方法叫做無損變換(unscented transformation)這一技術主要通過n個在先驗分布中采集的點(我們把它們叫sigma points)的線性回歸來線性化隨機變量的非線性函數,由于我們考慮的是隨機變量的擴展,所以這種線性化要比泰勒級數線性化(EKF所使用的策略)更準確。和EKF一樣,UKF也主要分為預測和更新

    運動模型

    本篇我們繼續使用CTRV運動模型,不了解該模型的同學可以去看上一篇博客,CTRV模型的具體形式如下: 

    在EKF中,我們將直線加速度和偏航角加速度的影響當作我們的處理噪聲,并且假設他們是滿足均值為 

    00,方差為 σaσa 和 σω˙σω˙,在這里,我們將噪聲的影響直接考慮到我們的狀態轉移函數中來。至于函數中的不確定項 μaμa 和 μω˙μω˙ 我們后面再分析。

    非線性處理/測量模型

    我們知道我們在應用KF是面臨的主要問題就是非線性處理模型(比如說CTRV)和非線性測量模型(RADAR測量)的處理。我們從概率分布的角度來描述這個問題:

    對于我們想要估計的狀態,在k時刻滿足均值為 xk|kxk|k ,方差為 Pk|kPk|k 這樣的一個高斯分布,這個是我們在k時刻的 后驗(Posterior)(如果我們把整個卡爾曼濾波的過程迭代的來考慮的話),現在我們以這個后驗為出發點,結合一定的先驗知識(比如說CTRV運動模型)去估計我們在 k+1k+1 時刻的狀態的均值和方差,這個過程就是卡爾曼濾波的預測,如果變換是線性的,那么預測的結果仍然是高斯分布,但是現實是我們的處理和測量模型都是非線性的,結果就是一個不規則分布,KF能夠使用的前提就是所處理的狀態是滿足高斯分布的,為了解決這個問題,EKF是尋找一個線性函數來近似這個非線性函數,而UKF就是去找一個與真實分布近似的高斯分布。

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

    那么如何去找一個與真實分布近似的高斯分布呢?——找一個與真實分布有著相同的均值和協方差的高斯分布。那么如何去找這樣的均值和方差呢?——無損變換。

    無損變換

    計算一堆點(術語叫做sigma point),通過一定的手段產生的這些sigma點能夠代表當前的分布,然后將這些點通過非線性函數(處理模型)變換成一些新的點,然后基于這些新的sigma點計算出一個高斯分布(帶有權重地計算出來)如圖:

    預測

    由一個高斯分布產生sigma point

    通常,假定狀態的個數為 nn ,我們會產生 2n+12n+1 個sigma點,其中第一個就是我們當前狀態的均值 μμ ,sigma點集的均值的計算公式為:

    其中的 λλ 是一個超參數,根據公式,λλ 越大, sigma點就越遠離狀態的均值,λλ 越小, sigma點就越靠近狀態的均值。需要注意的是,在我們的CTRV模型中,狀態數量 nn 除了要包含5個狀態以外,還要包含處理噪聲 μaμa 和 μω˙μω˙,因為這些處理噪聲對模型也有著非線性的影響。在增加了處理噪聲的影響以后,我們的不確定性矩陣 PP 就變成了:

    其中,PP′ 就是我們原來的不確定性矩陣(在CTRV模型中就是一個 5×55×5 的矩陣),Q是處理噪聲的協方差矩陣,在CTRV模型中考慮到直線加速度核Q的形式為:

    其中的 σ2aσa2 , σ2ω˙σω˙2 和我們上一篇博客講的一樣。以上公式中還存在一個問題,那就是矩陣開平方根怎么計算的問題,同產情況下,我們求得是:


    求解上式中的 AA 是一個相對復雜的過程,但是如果 PP 是對角矩陣的話,那么這個求解就會簡化,在我們的實例中,P表示對估計狀態的不確定性(協方差矩陣),我們會發現 PP 基本上是對角矩陣(狀態量之間的相關性幾乎為0), 所以我們可以首先對 PP 做 Cholesky分解(Cholesky decomposition) ,然后分解得到的矩陣的下三角矩陣就是我們要求的 AA ,在這里我們就不詳細討論了,在我們后面的實際例子中,我們直接調用庫中相應的方法即可(注意:本次的實例我們換C++來實現,相比于Python,C++更加貼近我們實際的開發):

    預測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實例

    我們繼續基于上一篇文章的數據來做狀態估計的實例,不過,為了更加接近實際實際的應用,我們本節采用C++實現。由于本節使用的C++代碼量相對較大,而且為多文件的項目,代碼就不再一一貼出,所有代碼我都已經上傳至如下地址:

    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,下面就讓我們看一下核心代碼。

    核心代碼

    首先是預測,預測主要包含三部分,分別是:

    • 產生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的核心算法實現了,可能光看這些核心代碼還是沒辦法理解,所以感興趣的童鞋就去下載來再詳細研究把~

    上一篇文章我買了個關子,沒有貼出把結果可視化的代碼,本次代碼已經一本包含在項目里(具體見/data/plot.py)

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


    顯示全部

    0個評論

      數據加載中...
    新疆11选五投注时间