https://github.com/min0jang/Kalman_Filter.git
MAIN FILES
main.cpp
: main running file
measurement_package.h
: class for processed data input
Tools.cpp
: includes functions calculating Root Mean Square Errors and a Jacobian Matrix
Tools.h
: header file of Tools.cpp
kalman_filter.cpp
: class and functions for general kalman filter calculation
kalman_filter.h
: header file of kalman_filter.cpp
Fusion.cpp
: class and functions that integrates the kalman filter calculation of LIDAR input data and RADAR input data
Fusion.h
: header file of Fusion.cpp
EIGEN/Dense
contains important classes like VectorXd and MatrixXd
PLOT FILES
data/obj_pose-laser-radar-synthetic-input.txt
: input data file, which I attained from Udacity github
data/output_rmse
: root mean square error for each step
data/output_radar
: radar measurements
data/output_lidar
: lidar measurements
data/output_predict
: predictions for each step
data/output_kf
: kalman filter - measurement update for each step
data/output_gt
: ground truth for rmse calculation
kf_live_plot.py
: reads in the output text files and live-plot the points
This is a simulation for Kalman Filter that tracks the position and velocity of a moving bike. Based on the sensor measurement received from LIDAR and RADAR, I calculated the "believed position and velocity" of the bike with Kalman Filter, and compared the result with corresponding ground truth by calculating root mean square errors.
obj_pose-laser-radar-synthetic-input.txt
contains data as following:
LIDAR measurement "L" (9 columns):
| sensor_type | x_measured | y_measured | timestamp | x_gt | y_gt | vx_gt | vy_gt | yaw_gt | yawrate_gt |
RADAR measurement "R" (10 columns):
| sensor_type | rho_measured | phi_measured | rhodot_measured | timestamp | x_gt | y_gt | vx_gt | vy_gt | yaw_gt | yawrate_gt |
Kalman filter is mainly comprised of 2 steps: predict and update. Prediction is done based on the most recent state of the object, which is the bike in this simulation. For example, if a bike was at x = 0 m, and it was moving at 1 me/s speed, we can easily predict that the bike will be at x = 1 m after one second. Then, the system receives the measurement from sensors(LIDAR and RADAR in this project), integrate the measurement with the predicted state from the first step, and update the position of the bike. The system repeats these two steps and tracks the position and velocity of the bike.
In this step, we have to predict the next position of the bike based on the current state. As you can tell from the trajectory(black line), the bike is moving to the right. There for, it is not too difficult to predict that the bike will move right for some distance. Also, we know that the velocity of the bike will not change very much in a short period of time (about 0.05 seconds here), so we predict the velocity to be about the same as the previous time step. Based on this information, we predict the next position of the bike to be this:
This prediction for next state can be easily calculated with a simple matrix multiplication. For example, px' = px + dx*vx, and vx' = vx.
Here, we define the state transition matrix 'F', which was multiplied to the state matrix for prediction, as following:
This matrix can also be used for state covariance update. If you would like to know more about "covariance" or any mathematical proof for this, look up "Gaussian Distribution for Kalman Filter".
Now, we need to calculate the position of the bike based on the received sensor data. In this simulation, it is either a LIDAR data or a RADAR data.
This time, we received a LIDAR measurement (yellow x-mark), which seems to be very accurate given that it is almost right on the current position of the bike (big red dot). Since we received a measurement, we need to update the position, which is calculated based on the most recently predicted position(yellow dot) and the sensor measurement(yellow x-mark).
The updated position is shown with a green dot.
Here are constant matrices for sensore measurement update calculation. Matrix 'R' is a measurement noise (or covariance) matrix, which reflects the possible noise of the sensor measurements. These values are usually provided by sensor manufacturers. Matrix 'H' is a measurement function matrix for measurement update calculation.
The reason we have different matrices for lidar and radar are because the measurement output for each sensor is different as discussed under "Simulation Setup" section. Following is the math done for calculation:
Here, matrix 'z' is a measurement matrix, matrix 'y' is called error matrix, and matrix 'K' is called Kalman gain.
The system repeats step 1 and step 2 until the simulation ends. The only thing that changes is that the type of measurement we receive varies: sometimes it is a LIDAR measurement, and somethimes it is a RADAR measurement. The math calculation for the two types of measurement are different, becausre LIDAR measurement outputs the position and velocity in cartesian coordinates while RADAR measurement outpus those in polar coordinates. This different methods are processed in Fusion.cpp
.
In order to figure out how accurate the system is working, I calculated the "Root Mean Square Error" for each time step. The equation of this error is: