- 2. Intro to Extended Kalman Filter Project
- 3. Data File for EKF project
- 4. File Structure
- 5. Main.cpp
- 6. Project Code
- 7. Tips and Tricks
- 8. Project Resources
- 9. Project Instructions for workspaces
2. Intro to Extended Kalman Filter Project
使用C++完成扩展的卡尔曼滤波器,通过提供的lidar,radar测量数据去检测汽车周边的自行车。
下面是通过模拟器输出的轨迹图:
- 红色表示激光雷达测量值
- 蓝色表示雷达测量值,有个小箭头表示方向
- 绿色三角形表示更新后的估算值
通过模拟器提供了测量数据给程序,程序反馈过来更新后的估算值和误差值。
3. Data File for EKF project
测量数据保存在obj_pose-laser-radar-synthetic-input.txt
文件中,文件截图如下:
第一行表示是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计算估算结果与实际结果的差值。
下面是主要的代码:
main.cpp
:从simulator方接受测量数据,调用函数实现卡尔曼滤波,最后评估计算RMSEFusionEKF.cpp
:初始化滤波器所需要参数,调用预测和更新函数kalman_filter.cpp
:实现预测和更新函数tools.cpp
:计算RMSE和Jacobian matrix
上面的2,3,4代码需要修改,下面是代码间的调用关系:
Main.cpp
读取数据,并将传感器测量值发送给FusionEKF.cpp
。FusionEKF.cpp
接收传感器数据,初始化变量以及更新变量。这里有个变量叫ekf_
,是KalmanFilter滤波器类的实例。ekf_
中包含了各种矩阵和向量数据,最后使用ekf_
来实现预测和更新操作。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
中完成预测和更新处理。
- Because lidar uses linear equations, the update step will use the basic Kalman filter equations.
- 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:
tools.cpp
: fill in the functions that calculate root mean squared error (RMSE) and the Jacobian matrix.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.kalman_filter.cpp
:fill out the Predict(), Update(), and UpdateEKF() functions.
8. Project Resources
9. Project Instructions for workspaces
- Navigate to the repository
CarND-Extended-Kalman-Filter-Project
using the file and directory menu on the left. - Run
./install-ubuntu.sh
(you may need to runchmod u+x install-ubuntu.sh
to make the file executable) - Complete the code
mkdir build && cd build
( from the project top directory)cmake .. && make
( build directory)./ExtendedKF
( build directory)
每次代码修改后,在build中执行上面的5和6,即能重新编译并运行。