This project involves the position estimation of vehicle and obstacles using Extended Kalman Filter and Particle Filter.
It is implemented in Matlab (R2019a) using the extendedKalmanFilter object and particleFilter object of the System Identification Toolbox.
The project is organised in three questions:
-
Question 1: Position estimation of moving vehicle & stationary obstacles using Extended Kalman Filter
-
Question 2: Position estimation of moving vehicle & stationary obstacles using Particle Filter
-
Question 3: Position estimation of moving vehicle & one moving obstacle Particle Filter
How to use:
-
For question 1, run
question1_plots_video.min Matlab -
For questions 2 & 3, run
question2.m&question3.mrespectively
(question1.m is used by question2.m & question3.m to first generate the estimations on which questions 2 & 3 rely)
Dataset 1: (used in Part 1 & 2)
- control1.csv
- radar1.csv
Dataset 2: (used in Part 3)
- control2.csv
- radar2.csv
control1.csv & control2.csv contain the speed measurements, u and θ
radar1.csv & radar2.csv contain the noisy measurement of the obstacles from the vehicle, d1, φ1, d2, φ2
Sampling: A sampling rate of 10Hz is assumed for both datasets.
Noise of measuring device: A mean value of 0 and a standard deviation of 0.3 radians (angle) and 0.5 meters (distance) are assumed.
| - src/
| - - myLikelihoodMeasurement2Fcn.m
| - - myLikelihoodMeasurementFcn.m
| - - myVehicleMovingObstacleStateTransitionFcn.m
| - - myVehicleStateTransitionFcn.m
| - - plot_error_covariance_ellipsoid.m
| - - question1_plots_video.m
| - - question1.m
| - - question2.m
| - - question3.m
| - datasets/
| - - control1.csv
| - - control2.csv
| - - radar1.csv
| - - radar2.csv
A vehicle us moving on a plane (2 dimensions). The vehicle is aware of two stationary obstacles on the same plane. The model of the movement of the vehicle is described by:
while the model of the measurement of the positions of the obstacles by:
and 
are the coordinates of the vehicle at time step t.
The noise in the system is Gaussian with mean vlaue 0 and standard deviation σ.
Task: Estimate the seven states using the Extended Kalman filter. The vehicle's measurements include the angle and distance from which it perceives each obstacle. The angle is calculated with respect to the longitudinal axis of the vehicle, with rotation considered counter-clockwise. Both angle and distance measurements are noisy, with Gaussian noise having a mean value of 0. The vehicle is subject to changing velocity and rotation.
Solution:
The model of the wolrd is described by:
f = @(x,u)[ x(1) + u(1)*cos(x(3))*dt;
x(2) + u(1)*sin(x(3))*dt;
x(3) + u(2)*dt;
x(4);
x(5);
x(6);
x(7)
];
x(1), x(2), x(3) describe the position of the vehicle and change according to the model of movement that was provided
x(4), x(5) are the coordinates of the first obstacle, and since it is stationary, they do not change
x(6), x(7) are the coordinates of the second obstacle, and, similarly to the first one, since it is stationary, they do not change.
The process noise was modeled as follows:
Q = [q1, 0, 0, 0, 0, 0, 0;
0, q2, 0, 0, 0, 0, 0;
0, 0, q3, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0];
The three first positions of the diagonal of the matrix describe the noise in the movement of the vehicle. The rest of the positions of the diagonal stay empty as they refer to the coordinates of the stationary obstacles and thus there can be no noise. It is assumed that q1=q2=q3 without loss of generality.
The model of the measurement for the obstacles is described by:
h= @(x) [sqrt((x(4)-x(1))^2 + (x(5) - x(2))^2);
atan2(((x(5) - x(2)),((x(4)-x(1))) - x(3);
sqrt((x(6)-x(1))^2 + (x(7) - x(2))^2);
atan2((x(7) - x(2)),(x(6)-x(1))) - x(3);
];
EKF - Prediction & Correction Steps:
kalmanFilterPredictionCorrection.mp4
EKF - Uncertainties of Positions:
uncertaintiesOfPositions.mp4
EKF - Corrected Vehicle Path and Covariance:
vehiclePath.Covariance.mp4
Task: Utilizing the best estimations of the obstacle positions obtained with the Extended Kalman filter (from question 1), use the Particle Filter to estimate the three states of the vehicle from the outset.
Best estimations -> final obstacle positions from the EKF. These are considered as fixed landmarks in this question.
PF - Prediction & Correction Steps:
particleFilterPredictionCorrection.mp4
Task: Assume that the second obstacle moves on the x-axis with an unknown stable velocity. Utilizing the best estimation from question 1 as the initial position of the second obstacle, estimate the three states of the vehicle and the position x of the moving obstacle using the Particle Filter. Utilize the provided noisy measurements (dataset 2).
Kalman Filter:
Extended Kalman Filter:
Particle Filter:
SLAM: