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

前面兩篇文章我們瞭解了卡爾曼濾波以及擴展卡爾曼濾波在目標追蹤的應用，我們在上一篇文章中還具體用Python實現了EKF，但是細心的同學會發現，EKF的效率確實很低，計算雅可比矩陣確實是一個很費時的操作，當問題（非線性的）一旦變得複雜，其計算量就變得十分不可控制。在此再向大家接受一種濾波——無損卡爾曼濾波（Unscented Kalman Filter， UKF）

創作不易，轉載請註明來源：http://blog.csdn.net/adamshan/article/details/78359048

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

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

運動模型

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

x \to (t + Δ t) = g (x (t)) = ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ v ω sin (ω Δ t + θ) - v ω sin (θ) + x (t) - v ω cos (ω Δ t + θ) + v ω sin (θ) + y (t) v ω Δ t + θ ω ⎞ ⎠ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ + ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ 1 2 Δ t 2 μ a cos (θ) 1 2 Δ t 2 μ a sin (θ) Δ t μ a 1 2 Δ t 2 μ ω ˙ Δ t μ ω ˙ ⎞ ⎠ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟, ω \neq 0

當偏航角爲0時：

x \to (t + Δ t) = g (x (t)) = ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ v cos (θ) Δ t + x (t) v sin (θ) Δ t + y (t) v ω Δ t + θ ω ⎞ ⎠ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ + ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ 1 2 Δ t 2 μ a cos (θ) 1 2 Δ t 2 μ a sin (θ) Δ t μ a 1 2 Δ t 2 μ ω ˙ Δ t μ ω ˙ ⎞ ⎠ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟, ω = 0

在EKF中，我們將直線加速度和偏航角加速度的影響當作我們的處理噪聲，並且假設他們是滿足均值爲 0 ，方差爲 σa 和 σω˙ ，在這裏，我們將噪聲的影響直接考慮到我們的狀態轉移函數中來。至於函數中的不確定項 μa 和 μω˙ 我們後面再分析。

非線性處理/測量模型

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

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

UKF的基本思路就是： 近似非線性函數的概率分佈要比近似非線性函數本身要容易！

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

無損變換

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

預測

由一個高斯分佈產生sigma point

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

χ [1] = μ

χ [i] = μ + ((n + λ) P - - - - - - - \sqrt) i f o r i = 2, . . ., n + 1

χ [i] = μ - ((n + λ) P - - - - - - - \sqrt) i - n f o r i = n + 2, . . ., 2 n + 1

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

P = (P' 0 0 Q)

其中，P′ 就是我們原來的不確定性矩陣（在CTRV模型中就是一個 5×5 的矩陣），Q是處理噪聲的協方差矩陣，在CTRV模型中考慮到直線加速度核Q的形式爲：

Q = [σ 2 a 0 0 σ 2 ω ˙]

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

A = P - - \sqrt

其中，

A A T = P

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

c++ MatrixXd A = P_aug.llt().matrixL();

預測sigma point

現在我們有sigma點集，我們就用非線性函數 g() 來進行預測：

χ k + 1 = g (χ k, μ k)

需要注意的是，這裏的輸入 χk 是一個 (7,15) 的矩陣（因爲考慮了兩個噪聲量），但是輸出 χk+1|k 是一個(5,15) 的矩陣（因爲這是預測的結果，本質上是基於運動模型的先驗，先驗中的均值不應當包含 a , ω˙ 這類不確定的量）

預測均值和方差

首先要計算出各個sigma點的權重，權重的計算公式爲：

w [i] = λ λ + n, i = 1

w [i] = 1 2 ( λ + n ), i = 2, . . ., 2 n + 1

然後基於每個sigma點的權重去求新的分佈的均值和方差：

μ' = \sum i = 1 2 n + 1 w [i] χ [i] k + 1

P' = \sum i = 1 2 n + 1 w [i] (χ [i] k + 1 - μ') (χ [i] k + 1 - μ') T

其中 μ′ 即爲我們基於CTRV模型預測的目標狀態的先驗分佈的均值 xk+1|k ，它是sigma點集中每個點各個狀態量的加權和， P′ 即爲先驗分佈的協方差（不確定性） Pk+1|k ，由每個sigma點的方差的加權和求得。至此，預測的部分也就走完了，下面進入了UKF的測量更新部分。

測量更新

預測測量（將先驗映射到測量空間然後算出均值和方差）

這篇博客繼續使用上一篇（EKF）中的測量實驗數據，那麼我們知道，測量更新分爲兩個部分，LIDAR測量和RADAR測量，其中LIDAR測量模型本身就是線性的，所以我們重點還是放在RADAR測量模型的處理上面，RADAR的測量映射函數爲：

Z k + 1 | k = ⎛ ⎝ ⎜ ρ ψ ρ ˙ ⎞ ⎠ ⎟ = ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ ⎜ x 2 + y 2 - - - - - - \sqrt a t a n 2 (y, x) v cos ( θ ) x + v sin ( θ ) y x 2 + y 2 - - - - - - \sqrt ⎞ ⎠ ⎟ ⎟ ⎟ ⎟ ⎟

這也是個非線性函數，我們用 h() 來表示它，再一次，我們使用無損轉換來解決，但是這裏，我們可以不用再產生sigma points了，我們可以直接使用預測出來的sigma點集，並且可以忽略掉處理噪聲部分。那麼對先驗的非線性映射就可以表示爲如下的sigma point預測（即預測非線性變換以後的均值和協方差）：

z k + 1 | k = \sum i = 1 2 n + 1 w [i] Z [i] k + 1 | k

S k + 1 | k = \sum i = 1 2 n + 1 w [i] (Z [i] k + 1 | k - z k + 1 | k) (Z [i] k + 1 | k - z k + 1 | k) T + R

和前面的文章一樣，這裏的 R 也是測量噪聲，在這裏我們直接將測量噪聲的協方差加到測量協方差上是因爲該噪聲對系統沒有非線性影響。在本例中，以RADAR的測量爲例，那麼測量噪聲R爲：

R = E [w w T] = ⎛ ⎝ ⎜ ⎜ σ 2 ρ 00 0 σ 2 ψ 0 00 σ 2 ρ ˙ ⎞ ⎠ ⎟ ⎟

更新狀態

首先計算出sigma點集在狀態空間和測量空間的互相關函數，計算公式如下：

T k + 1 | k = \sum i = 1 2 n + 1 w [i] (X [i] k + 1 | k - x k + 1 | k) (Z [i] k + 1 | k - z k + 1 | k) T

後面我們就完整地按照卡爾曼濾波的更新步驟計算即可，先計算卡爾曼增益：

K k + 1 | k = T k + 1 | k \cdot S - 1 k + 1 | k

更新狀態（也就是作出最後的狀態估計）：

x k + 1 | k + 1 = x k + 1 | k + K k + 1 | k (z k + 1 - z k + 1 | k)

其中 zk+1 是新得到的測量，而 zk+1|k 則是我們根據先驗計算出來的在測量空間的測量。

更新狀態協方差矩陣：

P k + 1 | k + 1 = P k + 1 | k - K k + 1 | k S k + 1 | k K T k + 1 | k

至此，UKF的完整理論部分就介紹完了，具體的無損變換的意義和推導由於太過於理論化了，大家如果感興趣可以去看這幾片文獻：

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 build
cd build
cmake ..
make

運行：

./ukf ../data/data_synthetic.txt ../data/output.txt

得到計算的結果：

Accuracy - RMSE:
0.0723408
0.0821208
 0.342265
  0.23017

我們發現，這個UKF要比我們上一篇博客寫的EKF的效率要高得多得多。

我們執行可視化的python腳本看一下效果：

cd ../data
python plot.py

得到如下結果：

放大一點：

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;
}

基於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;
    }
}

計算新的均值核方差

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();
    }
}

測量更新主要分爲：

預測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_;
}

預測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_;
}

更新狀態

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();
}

以上就是我們的UKF的核心算法實現了，可能光看這些核心代碼還是沒辦法理解，所以感興趣的童鞋就去下載來再詳細研究把～

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

前面三篇文章都介紹了卡爾曼濾波相關的算法，大家應該已經看出卡爾曼濾波這類貝葉斯濾波器在無人駕駛汽車系統哦的重要性了，卡爾曼濾波作爲重要的狀態估計算法，是無人駕駛的必備技能點，所以相關的擴展閱讀大家可以多多開展，本系列暫時就不再討論了，下面我想向大家介紹應用於無人駕駛中的控制算法，控制作爲自動化中重要的技術，有着很多的分支，後面的文章我會繼續由理論到實踐逐一向大家介紹相關技術～

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

運動模型

非線性處理/測量模型

無損變換

預測

由一個高斯分佈產生sigma point

預測sigma point

預測均值和方差

測量更新

預測測量（將先驗映射到測量空間然後算出均值和方差）

更新狀態

UKF實例

運行-效果

核心代碼

產生sigma點集

基於CTRV模型預測sigma點集

計算新的均值核方差

預測LIDAR測量

預測RADAR測量

更新狀態

[轉帖]使用NMT和pmap解決JVM資源泄漏問題原創

Python實現大麥網搶票的四大關鍵技術點解析

Python 安裝庫指令大全

salesforce零基礎學習（一百三十八）零碎知識點小總結（十）

一款開源的.NET程序集反編譯、編輯和調試神器

關於接口協議，你必須要知道這些！

基於 Milvus + LlamaIndex 實現高級 RAG

【2024-05-21】以茶會友

自動駕駛系統進階與項目實戰（五）使用SC-LEGO-LOAM進行較大規模點雲地圖構建和閉環優化

自動駕駛系統進階與項目實戰（四）自動駕駛高精度地圖構建中的三維場景識別和閉環檢測

Move-it無人駕駛黑客馬拉松記錄

自動駕駛系統進階與項目實戰（三）基於全卷積神經網絡的點雲三維目標檢測和ROS實戰

自動駕駛系統進階與項目實戰（二）多激光雷達外參自動化標定算法及代碼實例

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結