# Kidnapped Vehicle
#### Compiling
##### Code must compile without errors with cmake and make.
``` shell
Softwares-MacBook-Pro:tmp david$ git clone https://github.com/autohandle/CarNDKidnappedVehicleProject.git
Cloning into 'CarNDKidnappedVehicleProject'...
remote: Counting objects: 2622, done.
remote: Compressing objects: 100% (2483/2483), done.
remote: Total 2622 (delta 97), reused 2622 (delta 97), pack-reused 0
Receiving objects: 100% (2622/2622), 426.70 KiB | 1.30 MiB/s, done.
Resolving deltas: 100% (97/97), done.
Softwares-MacBook-Pro:tmp david$ cd CarNDKidnappedVehicleProject/
Softwares-MacBook-Pro:CarNDKidnappedVehicleProject david$ mkdir build
Softwares-MacBook-Pro:CarNDKidnappedVehicleProject david$ cd build
Softwares-MacBook-Pro:build david$ cmake ..
-- The C compiler identification is AppleClang 9.0.0.9000037
-- The CXX compiler identification is AppleClang 9.0.0.9000037
-- Check for working C compiler: /Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/cc
-- Check for working C compiler: /Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/c++
-- Check for working CXX compiler: /Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Configuring done
-- Generating done
-- Build files have been written to: /tmp/CarNDKidnappedVehicleProject/build
Softwares-MacBook-Pro:build david$ make
Scanning dependencies of target particle_filter
[ 33%] Building CXX object CMakeFiles/particle_filter.dir/src/particle_filter.cpp.o
[ 66%] Building CXX object CMakeFiles/particle_filter.dir/src/main.cpp.o
[100%] Linking CXX executable particle_filter
ld: warning: directory not found for option '-L/usr/local/Cellar/libuv/1.11.0/lib'
[100%] Built target particle_filter
Softwares-MacBook-Pro:build david$ ./particle_filter
Listening to port 4567
Connected!!!
data: <2훼��vcket.io/?EIO=4&transport=websocket HTTP/1.1
user-agent: websocket-sharp/1.0
upgrade: websocket
connection: Upgrade
host: 127.0.0.1:4567
sec-websocket-key: WpqvzQDcW0NqKBrKQHllvQ==
sec-websocket-version: 13
>
```
#### Accuracy
##### The output says "Success! Your particle filter passed!"
![Your Particle Filter Passed](./images/YourParticleFilterPassed.png)
#### Performance
##### Runs within the specified time of 100 seconds
The simulation shows a completion time of: 64 seconds. I ran the simulation several times and the maximum finishing time was 95 seconds — that worst-case performance can be viewed in the [video](https://s3.amazonaws.com/autohandle.com/video/CarNDKidnappedVehicleProject.mp4).
#### General
##### Code uses a particle filter and follows the Correct Algorithm
The algorithm is implemented in
[ParticleFilter.cpp](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/master/src/particle_filter.cpp) and [ParticleFIlter.h](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/master/src/particle_filter.h).
###### [ParticleFilter::init](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L23-L49)
The main program first initializes the [ParticleFilter](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/master/src/particle_filter.cpp) in [ParticleFilter::init](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L23-L49) with 500 particles in a random state (x,y, and theta) around an initial x,y gps reading,
``` C++
for (int particle = 0; particle < num_particles; ++particle) {
double sample_x, sample_y, sample_theta;
// TODO: Sample and from these normal distrubtions like this:
// sample_x = dist_x(gen);
// where "gen" is the random engine initialized earlier.
sample_x=dist_x(gen);
sample_y=dist_y(gen);
sample_theta=dist_theta(gen);
addParticleToFilter(createParticle(sample_x, sample_y, sample_theta));
}
is_initialized=true;
```
then the `is_initialized` flag is set so that the main program does not try to initialize [ParticleFilter](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/master/src/particle_filter.cpp) again.
After initialization, the main program loops through: [ParticleFilter::prediction](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L58-L98), [ParticleFilter::updateWeights](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L145-L183), and [ParticleFilter::resample](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L185-L240).
###### [ParticleFilter::prediction](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L58-L98)
[ParticleFilter::prediction](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L58-L98) loops through every particle in the [ParticleFilter](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/master/src/particle_filter.cpp) updating its state. If the `yaw_rate` is 0, then only the x,y position is updated using the current velocity and time step
``` c++
const double deltaV = delta_t*velocity;
xf = x0 + deltaV*cos(theta0);
yf = y0 + deltaV*sin(theta0);
thetaf = theta0;
```
otherwise, the `yaw_rate` is used to update all three state variables
``` C++
xf = x0 +(velocity/yaw_rate)*(sin(theta0+yaw_rate*delta_t)-sin(theta0));
yf = y0 +(velocity/yaw_rate)*(cos(theta0)-cos(theta0+yaw_rate*delta_t));
thetaf = theta0+yaw_rate*delta_t;
```
In both cases, noise is added to all the state variables
``` C++
normal_distribution<double> noisyXf(xf, std_pos[0]);
normal_distribution<double> noisyYf(yf, std_pos[1]);
normal_distribution<double> noisyThetaf(thetaf, std_pos[2]);
particle.x=noisyXf(gen);
particle.y=noisyYf(gen);
assert(!std::isnan(particle.y) && !std::isnan(particle.y));
particle.theta=noisyThetaf(gen);
```
After the particle states have been updated, the accuracy/weight of each particle at the new/predicted location can be calculated.
###### [ParticleFilter::updateWeights](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.cpp#L145-L183)
First, the list of all landmarks is reduced in [filterLandmarkMap](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.h#L94-L108) by using the `sensor_range` and the particle's new/predicted location
```C++
if (fabs(fabs(landmarkInMap.x_f)-fabs(theParticle.x))<theSensorRange && fabs(fabs(landmarkInMap.y_f)-fabs(theParticle.y))<theSensorRange) {
LandmarkObs landmarkInRange;
landmarkInRange.id=landmarkInMap.id_i;
landmarkInRange.x=landmarkInMap.x_f;
landmarkInRange.y=landmarkInMap.y_f;
landmarksInSensorRange.push_back(landmarkInRange);
}
```
Then, assuming each particle is where the current `observations` were actually made, the `observations` vector is transformed into map coordinates by using each particle x,y,theta position in [transform](https://github.com/autohandle/CarNDKidnappedVehicleProject/blob/24502292382ccf2178b8a5f79b45967ffa671ea8/src/particle_filter.h#L45-L66) to translate and rotate each observation.
``` C++
LandmarkObs* const inMapCoordinates = new LandmarkObs