This project involves the position estimation of vehicle and obstacles using the Extended-Kalman and Particle filters.
It is implemented in Matlab (R2019a) using the:
- extendedKalmanFilter object and
- particleFilter object
of the System Identification Toolbox.
The project is organized in three parts:
-
Part 1: Position estimation of moving vehicle & static obstacles using Extended Kalman Filter
-
Part 2: Position estimation of moving vehicle & static obstacles using Particle Filter
-
Part 3: Position estimation of moving vehicle & one moving obstacle Particle Filter
How to use:
-
For part 1, just run
question1_plots_video.min Matlab -
For part 2,
question2.m -
For part 3,
question3.m
(question1.m is used by question2.m & question3.m to first generate the estimations on which part 2 & part 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 static 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 static, they do not change
x(6), x(7) are the coordinates of the second obstacle, and, similarly to the first one, since it is static, 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 static 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);
];
Extended Kalman Filter: Prediction & Correction Steps
kalmanFilterPredictionCorrection.mp4
Task: Utilizing the best estimations of the obstacle positions obtained with the Extended Kalman filter (from part 1), use the Particle Filter to estimate the three states of the vehicle from the outset.
Task: Assume that the second obstacle moves on the x-axis with an unknown stable velocity. Utilizing the best estimation from part 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: