1200字范文 > 曲线平滑/散点拟合——卡尔曼滤波

曲线平滑/散点拟合——卡尔曼滤波

时间：2020-01-06 14:48:36

相关推荐

曲线平滑/散点拟合——卡尔曼滤波

1. 初始化一系列的点2.变量3. 迭代过程4. 结果

Date: /04/15
Editor:萧潇子（Jesse）
Contact: 1223167600@

由于工作需要,这几日做了根据视觉车道线曲率限速的事情,由于视觉车道线检测的抖动,由此带来的车道线曲率特性在时间域上存在不小的抖动,并且影响无人车乘客的体验,借此机会探索了相关滤波算法的功能以及应用场景,但是关于卡尔曼滤波始终没有找到在跟踪道路曲率怎么使用,因此凭借兴趣研究了一下使用卡尔曼滤波平滑曲线的想法,本文基于对车道线的平滑尝试使用卡尔曼滤波做道路散点的拟合和平滑,以下是整个过程:

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.变量

多项式系数 C C C作为系统状态变量,假设状态变量之间不相关散点X坐标作为观测变量,Y坐标作为期望均值传感器噪声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;}