曲線平滑/散點擬合——卡爾曼濾波

---

Date: 2020/04/15
Editor:蕭瀟子（Jesse）
Contact: [email protected]

---

由於工作需要,這幾日做了根據視覺車道線曲率限速的事情,由於視覺車道線檢測的抖動,由此帶來的車道線曲率特性在時間域上存在不小的抖動,並且影響無人車乘客的體驗,藉此機會探索了相關濾波算法的功能以及應用場景,但是關於卡爾曼濾波始終沒有找到在跟蹤道路曲率怎麼使用,因此憑藉興趣研究了一下使用卡爾曼濾波平滑曲線的想法,本文基於對車道線的平滑嘗試使用卡爾曼濾波做道路散點的擬合和平滑,以下是整個過程:

1. 初始化一系列的點

首先我們使用多項式方程產生一系列初始點,並且在初始點的位置上給定一部分上下的隨機擾動,模擬真實情況下視覺車道線或者高精度地圖質量不高的點.

    /*
     * @ X Y 初始化系列點
     */

    Eigen::VectorXd X;
    Eigen::VectorXd Y;
    X.setLinSpaced(N, 0, 10);
    Y.resize(N);

    std::vector<double> X_;
    std::vector<double> Y_;
    std::vector<double> YR;

    srand((unsigned)time(NULL));
    for (int i = 0; i < N; i++)
    {

        int a = -100;
        int b = 100;
        double random = (rand() % (b - a + 1)) + a;
        Y[i] = std::pow(X[i], 3); //std::pow(X[i], 3) + random;
        X_.push_back(X[i]);
        Y_.push_back(Y[i]);
        YR.push_back(Y[i] + random);
    }

2.變量

1. 多項式係數$C$作爲系統狀態變量,假設狀態變量之間不相關
2. 散點X座標作爲觀測變量,Y座標作爲期望均值
3. 傳感器噪聲R 干擾Q

    /*
     * @ H(N,Order+1)
     * 
     *   |x^(n), x^(n-1),...,x^2,x^1|
     *   |                          |
     *   |                          |
     *   |x^(n), x^(n-1),...,x^2,x^1|
     * 
     * @H觀測變量矩陣 Y = C*H
     * 
     */

    Eigen::MatrixXd H;
    H.resize(N, Order + 1);
    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < Order + 1; j++)
        {
            H(i, j) = std::pow(X[i], Order + 1 - j);
        }
    }

    std::vector<Eigen::VectorXd> C_E;
    C_E.resize(N);
    Eigen::VectorXd C_E_0 = Eigen::VectorXd::LinSpaced(Order + 1, 0, 0);
    C_E[0] = C_E_0;

    std::vector<Eigen::MatrixXd> P_E;
    P_E.resize(N);
    Eigen::MatrixXd P_E_0;
    P_E_0.setIdentity(Order + 1, Order + 1);
    P_E[0] = P_E_0;

    double Q = 0; //噪聲
    double R = 0; //測量誤差

3. 迭代過程

具體迭代過程如下:

    /*
     * @ Kalman Iteration
     */

    for (int i = 1; i < N; i++)
    {
        Eigen::VectorXd c = C_E[i - 1];
        Eigen::MatrixXd p = P_E[i - 1].array() + Q;
        Eigen::VectorXd KK = (p * H.row(i).transpose()).array() / ((H.row(i) * p * H.row(i).transpose() + R));
        C_E[i] = c + KK * (Y(i) - H.row(i) * c);
        P_E[i] = (Eigen::MatrixXd::Identity(Order + 1, Order + 1) - KK * H.row(i)) * p;
    }

4. 結果

給出仿真實例如下圖所示,圖中青色點代表需要擬合平滑的原始點,品紅色代表對曲線係數使用卡爾曼濾波之後擬合曲線上的點,從結果不難看出,在合理範圍內的抖動使用這種方法是可以解決問題的