Haste makes waste

Nano01(自動運転)-U05-Lesson26-Project:Extended Kalman Filters Project

Posted on By lijun

2. Intro to Extended Kalman Filter Project

使用C++完成扩展的卡尔曼滤波器,通过提供的lidar,radar测量数据去检测汽车周边的自行车。

下面是通过模拟器输出的轨迹图:

  1. 红色表示激光雷达测量值
  2. 蓝色表示雷达测量值,有个小箭头表示方向
  3. 绿色三角形表示更新后的估算值

通过模拟器提供了测量数据给程序,程序反馈过来更新后的估算值和误差值。

image

3. Data File for EKF project

测量数据保存在obj_pose-laser-radar-synthetic-input.txt文件中,文件截图如下:

image

第一行表示是lidar(激光雷达),或是radar(雷达)。

radar时,各个行分别表示:

  • sensor_type
  • rho_measured
  • phi_measured
  • rhodot_measured
  • timestamp
  • x_groundtruth
  • y_groundtruth
  • vx_groundtruth
  • vy_groundtruth
  • yaw_groundtruth
  • yawrate_groundtruth

lidar时,各个行分别表示:

  • sensor_type
  • x_measured
  • y_measured
  • timestamp
  • x_groundtruth
  • y_groundtruth
  • vx_groundtruth
  • vy_groundtruth
  • yaw_groundtruth
  • yawrate_groundtruth

数据读取:

main.cpp中:

MeasurementPackage meas_package;
meas_package.sensor_type_ = MeasurementPackage::LASER;
meas_package.raw_measurements_ = VectorXd(2);
meas_package.raw_measurements_ << px, py;
meas_package.timestamp_ = timestamp;
vector<VectorXd> ground_truth;
VectorXd gt_values(4);
gt_values(0) = x_gt;
gt_values(1) = y_gt; 
gt_values(2) = vx_gt;
gt_values(3) = vy_gt;
ground_truth.push_back(gt_values);

4. File Structure

卡尔曼滤波器主要包含了三个步骤:

  • 初始化卡尔曼滤波器的变量
  • 预测:在经过△t时间后,物体的状态
  • 更新:基于传感器的测量值,更新物体的状态值

然后就是预测和更新的不断循环,另外,为了衡量卡尔曼滤波器的性能,通过RMSE计算估算结果与实际结果的差值。

下面是主要的代码:

  1. main.cpp:从simulator方接受测量数据,调用函数实现卡尔曼滤波,最后评估计算RMSE
  2. FusionEKF.cpp:初始化滤波器所需要参数,调用预测和更新函数
  3. kalman_filter.cpp:实现预测和更新函数
  4. tools.cpp:计算RMSE和Jacobian matrix

上面的2,3,4代码需要修改,下面是代码间的调用关系:

  1. Main.cpp读取数据,并将传感器测量值发送给FusionEKF.cpp
  2. FusionEKF.cpp接收传感器数据,初始化变量以及更新变量。这里有个变量叫ekf_,是KalmanFilter滤波器类的实例。ekf_中包含了各种矩阵和向量数据,最后使用ekf_来实现预测和更新操作。
  3. KalmanFilter,包含了预测和更新函数。

5. Main.cpp

虽然不需要修改Main.cpp文件,但是了解其结构有助于项目的进展。

simulator是一个客户端,这个C++程序是个web server。 h.onMessage接收客户端的输入并相应,将对应的数据保存到对应变量meas_package中。

6. Project Code

FusionEKF.cpp:

这个类中,需要初始化变量,初始化卡尔曼滤波器,然后调用预测和更新函数。

You will need to:

  • initialize variables and matrices (x, F, H_laser, H_jacobian, P, etc.)
  • initialize the Kalman filter position vector with the first sensor measurements
  • modify the F and Q matrices prior to the prediction step based on the elapsed time between measurements
  • call the update step for either the lidar or radar sensor measurement. Because the update step for lidar and radar are slightly different, there are different functions for updating lidar and radar.

对应代码:

  // initializing matrices
  R_laser_ = MatrixXd(2, 2);
  R_radar_ = MatrixXd(3, 3);
  H_laser_ = MatrixXd(2, 4);
  Hj_ = MatrixXd(3, 4);

  //measurement covariance matrix - laser
  R_laser_ << 0.0225, 0,
              0, 0.0225;

  //measurement covariance matrix - radar
  R_radar_ << 0.09, 0, 0,
              0, 0.0009, 0,
              0, 0, 0.09;

  /**
   * TODO: Finish initializing the FusionEKF.
   * TODO: Set the process and measurement noises
   */

如果是第一次,则需要用传感器测量值初始化物体的位置:

  /**
   * Initialization
   */
  if (!is_initialized_) {
    /**
     * TODO: Initialize the state ekf_.x_ with the first measurement.
     * TODO: Create the covariance matrix.
     * You'll need to convert radar from polar to cartesian coordinates.
     */

    // first measurement
    cout << "EKF: " << endl;
    ekf_.x_ = VectorXd(4);
    ekf_.x_ << 1, 1, 1, 1;

    if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
      // TODO: Convert radar from polar to cartesian coordinates 
      //         and initialize state.

    }
    else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
      // TODO: Initialize state.

    }

    // done initializing, no need to predict or update
    is_initialized_ = true;
    return;
  }

FusionEKF.cpp中有个变量ekf_,它是KalmanFilter类的示例,存储了卡尔曼对应的系数,并利用它调用预测和更新函数。

KalmanFilter Class

需要在kalman_filter.cpp中完成预测和更新处理。

  1. Because lidar uses linear equations, the update step will use the basic Kalman filter equations.
  2. radar uses non-linear equations, so the update step involves linearizing the equations with the Jacobian matrix.

Tools.cpp

用于计算root mean squared error 和 the Jacobian matrix。

7. Tips and Tricks

Summary of What Needs to Be Done:

  1. tools.cpp: fill in the functions that calculate root mean squared error (RMSE) and the Jacobian matrix.
  2. FusionEKF.cpp:You’ll need to initialize the Kalman Filter, prepare the Q and F matrices for the prediction step, and call the radar and lidar update functions.
  3. kalman_filter.cpp:fill out the Predict(), Update(), and UpdateEKF() functions.

8. Project Resources

9. Project Instructions for workspaces

  1. Navigate to the repository CarND-Extended-Kalman-Filter-Project using the file and directory menu on the left.
  2. Run ./install-ubuntu.sh(you may need to run chmod u+x install-ubuntu.sh to make the file executable)
  3. Complete the code
  4. mkdir build && cd build ( from the project top directory)
  5. cmake .. && make ( build directory)
  6. ./ExtendedKF ( build directory)

每次代码修改后,在build中执行上面的5和6,即能重新编译并运行。