Vehicle Steering Control using MPC. Adding files for project submission

CMakeLists.txt

@@ -0,0 +1,29 @@
+project(MPC)
+
+cmake_minimum_required (VERSION 3.5)
+
+# -g allows for gdb debugging
+# turn on -03 for best performance
+add_definitions(-std=c++11 -O3)
+
+set(CXX_FLAGS "-Wall")
+set(CMAKE_CXX_FLAGS "${CXX_FLAGS}")
+
+set(sources src/MPC.cpp src/main.cpp)
+
+include_directories(/usr/local/include)
+link_directories(/usr/local/lib)
+include_directories(src/Eigen-3.3)
+
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+
+include_directories(/usr/local/opt/openssl/include)
+link_directories(/usr/local/opt/openssl/lib)
+link_directories(/usr/local/Cellar/libuv/1*/lib)
+
+endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+
+add_executable(mpc ${sources})
+
+target_link_libraries(mpc ipopt z ssl uv uWS)
+

DATA.md

@@ -0,0 +1,41 @@
+# Websocket Data
+
+This document describes the JSON object send back from the simulator command server.
+
+Fields:
+
+* `ptsx` (Array<float>) - The global x positions of the waypoints.
+* `ptsy` (Array<float>) - The global y positions of the waypoints. This corresponds to the z coordinate in Unity
+since y is the up-down direction.
+* `psi` (float) - The orientation of the vehicle in **radians** converted from the Unity format to the standard format expected in most mathemetical functions (more details below).
+* `psi_unity` (float) - The orientation of the vehicle in **radians**. This is an orientation commonly used in [navigation](https://en.wikipedia.org/wiki/Polar_coordinate_system#Position_and_navigation).
+* `x` (float) - The global x position of the vehicle.
+* `y` (float) - The global y position of the vehicle.
+* `steering_angle` (float) - The current steering angle in **radians**.
+* `throttle` (float) - The current throttle value [-1, 1].
+* `speed` (float) - The current velocity in **mph**.
+
+
+### `psi` and `psi_unity` representations
+
+`psi`
+
+```
+//            90
+//
+//  180                   0/360
+//
+//            270
+```
+
+
+`psi_unity`
+
+```
+//            0/360
+//
+//  270                   90
+//
+//            180
+```
+

README.md

@@ -1,2 +1,101 @@
-# Vehicle-Steering-Control-using-MPC
+# **Vehicle Control Using Model Predictive Controller "MPC"**
+---
+
+**In this project, I have worked on a Model Predictive controller "MPC" to control the vehicle's steering angle .**
+
+## Project Introduction
+---
 In this project, we are controlling the vehicle's steering angle to be within the road using Model Predictive Controller "MPC".
+
+## Model Discussion
+---
+In the following, I will be discussing the model used through the project
+
+### What is Model Predictive Cpontroller "MPC" ? 
+---
+One approach to reach a good controller is to fisrt model the vehicle dynamics and constraints. This way we can analyse and tune the controller more efficiently.
+
+The models describe mathematically how the vehicle moves. Some models are more realistic than others. Typically, more realistic models are more complex and more challenging to work with.
+
+MPC reframes the control problem of following a trajectory as an optimization problem. The solution to the optimization problem is the optimal trajectory. MPC involves simulating different actuator inputs, predicting resulting trajectory, and selecting the trajectory with a minimum cost. We are constantly calculating inputs over a future horizon. That is why this approach is sometimes called **reducing horizon control**
+
+### Model Definition
+---
+During this project, we will use a relatively simple type of model called the **kinematic model**
+
+Kinematic models ignore many elements like gravity and tire forcesm but they are relatively simple to work with and they often perform well. Actually, kinematic models are simplification of dynamic models. Dynamic models aim to embody the actual vehicle dynamics as closely as possible. This might encompass tire forces, longitudinal and lateral forces, interia, gravity, air resistance, mass, ... etc
+
+The used model within our project is defined in the following figure: 
+
+![model](https://i.imgur.com/AMBb6nm.png)
+
+- The vehicle is located at a positionwe track with `x` and `y`.
+- Also, we need orientation which we denote with `psi`
+- As the vehicle is moving, so it will have velocity `v`
+- Inputs allow to control the state of the vehicle. Most cars have three actuators: steering, throttle `delta`, and brake peddals. For simplicity, we will consider the throttle and brake as a single actuator `a` where +ve values denote for accelerating and -ve value denote for braking.
+- `Lf` measure the distance between the center of mass of the vehicle and its front axle.
+- We want to minimize the predicated distance of the vehicle from the trajectory. That what we call **Cross Track Error** `cte`. In our case `cte` can be defined as the difference between the trajectory line and the current vehicle `y` position. Actually here we will not assume the reference trajectory as a line, *we will consider it as a third degree polynomial were most roads fit with it*.
+- We want also to minimize the predicated differnece in angle between the vehicle orientation and the trajectory orientation. We call this psi error `epsi`
+- `psides` is the desired orientation
+
+### Cost Function
+---
+In order to develop and optimal controller, we need to define a cost function that captures the errors we want to minimize. The cost is actually the difference between where we want the vehicle to go and where it predicted it will go based on the model. Ideally for our cost function, we want to have both `cte` and `epsi` to be zero.
+
+![cost](https://i.imgur.com/NKaW8xt.png)
+
+## Prediction Horizon and Letency
+---
+The prediction horizon is the duration over which future prediction are made. This duration is defined by the product of  two parameters
+- The number of time steps in the horizon `N`
+- How much time elapses between actuations `dt`
+
+`N` and `dt` are hyperparameters that I have tuned multiple times during my project development. The general rule for tuning these values is to have `dt` small as much as possible, with `N` that makes the prediction horizon as large as possible. The prediction horizon could be few seconds at most as beyond the prediction horizon the environment will change that it would not make sense to predict any further into the future. Also, MPC attempts to approximate a continuous referece trajectory by mean of the discrete paths between actuations. large values of `dt` result in loss of frequest actuaiotns which makes it harder to accurately approximate a continuous referece trajectory. This is sometimes called "discretization error".
+
+So, I have chosen `dt = 0.1` and `N = 10` which results to a prediction horizon of `1 second` in the future. That resulted to an efficient trajectory prediction.
+
+For targetting a real-life controller, in which the actuations do not apply immediately (not like simulation). So, a latency factor of 100 ms is applied. The purpose is to mimic real driving conditions where the car does actuate the commands instantly.
+Dealing with latency is done by a thread that run every 100 ms. 
+
+## Polynomial Fitting
+---
+We have mentioned in the model definition section that `cte` can be defined as the difference between the trajectory line and the current vehicle `y` position. Actually here we will not assume the reference trajectory as a line:
+> **we use a third degree polynomial where most roads fit with it**.
+
+The x and y points returned from the simulator are transformed to the vehicle coordinates as it simplifies the calculations
+
+
+## Constraints
+---
+Physically, a vehicle can not make right angle turns. So, the steering angle is limited within -25 and +25 degrees which are -/+0.33 radians.
+
+## Environment:
+---
+* Ubuntu 16.04 LTS
+* Udacity Self-Driving Car Nano-Degree Term2 Simulator
+* cmake >= 3.5
+* make >= 4.1
+* gcc/g++ >= 5.4
+
+## Running the Code
+---
+This project involves the Term 2 Simulator which can be downloaded [here](https://github.com/udacity/self-driving-car-sim/releases)
+
+This repository includes two files that can be used to set up and install uWebSocketIO for either Linux or Mac systems. For windows you can use either Docker, VMware, or even Windows 10 Bash on Ubuntu to install uWebSocketIO.
+
+Once the install for uWebSocketIO is complete, the main program can be built and ran by doing the following from the project top directory.
+
+1. mkdir build
+2. cd build
+3. cmake ..
+4. make
+5. ./mpc
+
+## Conclusion
+---
+  * Model Predictive Controller "MPC" is one of the powerful and efficient tools that can be used in autonomous driving
+  * MPC takes into consideration the state and the control input for the vehicle.
+  * MPC model the vehcile dynamics and predicts in future how the vehicle will be.
+  * MPC overcomes PID as MPC can apadt quite well because of the model, and taking the latency into consideration.
+  * [Here](https://youtu.be/ojT-lh0apDg) you can find a demo video for the MPC project.
+

cmakepatch.txt

@@ -0,0 +1,21 @@
+--- CMakeLists.txt	2017-03-23 20:58:47.000000000 +0900
++++ CMakeListsnew.txt	2017-03-23 20:57:33.000000000 +0900
+@@ -32,10 +32,16 @@
+ target_link_libraries (uWS LINK_PUBLIC ${OPENSSL_CRYPTO_LIBRARY})
+ target_link_libraries (uWS LINK_PUBLIC ${ZLIB_LIBRARY})
+
+-if (UNIX)
++if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 
+ target_link_libraries (uWS LINK_PUBLIC pthread)
+ install (TARGETS uWS DESTINATION /usr/lib64)
+ install (FILES src/Extensions.h src/WebSocketProtocol.h src/Networking.h src/WebSocket.h src/Hub.h src/Group.h src/Node.h src/Socket.h src/HTTPSocket.h src/uWS.h src/uUV.h DESTINATION /usr/include/uWS)
+-endif (UNIX)
++endif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 
++
++if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 
++target_link_libraries (uWS LINK_PUBLIC pthread)
++install (TARGETS uWS DESTINATION /usr/local/lib)
++install (FILES src/Extensions.h src/WebSocketProtocol.h src/Networking.h src/WebSocket.h src/Hub.h src/Group.h src/Node.h src/Socket.h src/HTTPSocket.h src/uWS.h src/uUV.h DESTINATION /usr/local/include/uWS)
++endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 
+
+ add_subdirectory(examples)

install-mac.sh

@@ -0,0 +1,16 @@
+#! /bin/bash
+brew install openssl libuv cmake zlib
+git clone https://github.com/uWebSockets/uWebSockets 
+cd uWebSockets
+git checkout e94b6e1
+patch CMakeLists.txt < ../cmakepatch.txt
+mkdir build
+export PKG_CONFIG_PATH=/usr/local/opt/openssl/lib/pkgconfig 
+cd build
+OPENSSL_VERSION=`openssl version -v | cut -d' ' -f2`
+cmake -DOPENSSL_ROOT_DIR=$(brew --cellar openssl)/$OPENSSL_VERSION -DOPENSSL_LIBRARIES=$(brew --cellar openssl)/$OPENSSL_VERSION/lib ..
+make 
+sudo make install
+cd ..
+cd ..
+sudo rm -r uWebSockets

install-ubuntu.sh

@@ -0,0 +1,14 @@
+#! /bin/bash
+sudo apt-get update
+sudo apt-get install git libuv1-dev libssl-dev gcc g++ cmake make
+git clone https://github.com/uWebSockets/uWebSockets 
+cd uWebSockets
+git checkout e94b6e1
+mkdir build
+cd build
+cmake ..
+make 
+sudo make install
+cd ../..
+sudo ln -s /usr/lib64/libuWS.so /usr/lib/libuWS.so
+sudo rm -r uWebSockets

install_Ipopt_CppAD.md

@@ -0,0 +1,56 @@
+## Intalling Ipopt and CppAD
+
+### Dependencies
+
+At this point in the curriculum students will have set up their SDC Term 2 environment and dependencies, with the exception of Ipopt, Fortran, and CppAD.  If you are setting up a fresh environment please refer to setup instructions starting [here](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/382ebfd6-1d55-4487-84a5-b6a5a4ba1e47).
+
+### Installation Process
+
+1.  Clone this repository and navigate to the cloned directory
+2.  [Download](https://www.coin-or.org/download/source/Ipopt/) the appropriate version of Ipopt (3.12.7 or higher) from the link below.  You may also use wget or a similiar command to download the source from the command line (see Linux instructions).
+3.  Follow the instructions for your environment
+
+* [Ipopt](https://projects.coin-or.org/Ipopt)
+  * **Mac:**
+    ```
+      brew tap brewsci/science
+      brew install ipopt --with-openblas
+    ```
+    Alternatively, you may be able to install directly from this repository with the below, although you may be missing certain dependencies that need to be carried over from the above:
+    ```
+    brew tap udacity/CarND-MPC-Project https://github.com/udacity/CarND-MPC-Project
+    brew install ipopt --with-openblas
+    ```
+
+ - **For Linux and Windows Ubuntu BASH** Please note that for any particular command, including execution of ```.sh``` scripts, it may be necessary to add ```sudo``` prior to the command.  It is also a good practice to run ```sudo apt-get update``` prior to installation of new libraries.
+
+  * **Linux:**
+    * ```sudo apt-get install gfortran```
+    *  ```apt-get install unzip```
+    * ```wget https://www.coin-or.org/download/source/Ipopt/Ipopt-3.12.7.zip && unzip Ipopt-3.12.7.zip && rm Ipopt-3.12.7.zip```
+    * Call `install_ipopt.sh` with the source directory as the first argument, ex: ```./install_ipopt.sh Ipopt-3.12.7``` or ```bash install_ipopt.sh Ipopt-3.12.7```
+
+  * **Windows:** For Windows environments there are two main options
+    * Follow Linux instructions in the Ubuntu Bash environment. Please not that install instructions should be executed from the repository directory.  Changing to a Windows directory (ie ```cd /mnt/c .....```) can result in installation issues, particularly for Windows directories that contain spaces.
+    * Use the docker container described [here](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/16cf4a78-4fc7-49e1-8621-3450ca938b77), which comes pre-configured with Ipopt.
+* [CppAD](https://www.coin-or.org/CppAD/)
+  * Mac: `brew install cppad`
+  * Linux `sudo apt-get install cppad` or equivalent.
+  * **Windows:** For Windows environments there are two main options
+    * Follow Linux instructions in the Ubuntu Bash environment
+    * Use the docker container described [here](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/16cf4a78-4fc7-49e1-8621-3450ca938b77), which comes pre-configured with CppAD.
+
+### Troubleshooting
+
+* If challenges to installation are encountered (install script fails).  Please consult the forums.  Please feel free to submit additional tips or forum threads to the [issue reports repo](https://github.com/udacity/sdc-issue-reports), for potential inclusion in this document.
+*  **Some Mac users have experienced the following error:**
+     ```
+     Listening to port 4567
+     Connected!!!
+     mpc(4561,0x7ffff1eed3c0) malloc: *** error for object 0x7f911e007600: incorrect checksum for freed object
+     - object was probably modified after being freed.
+     *** set a breakpoint in malloc_error_break to debug
+     ```
+     This error has been resolved by updrading ipopt with
+     ```brew upgrade ipopt --with-openblas```
+     per this [forum post](https://discussions.udacity.com/t/incorrect-checksum-for-freed-object/313433/19)

install_ipopt.sh

@@ -0,0 +1,45 @@
+# Pass the Ipopt source directory as the first argument
+if [ -z $1 ]
+then
+    echo "Specifiy the location of the Ipopt source directory in the first argument."
+    exit
+fi
+cd $1
+
+prefix=/usr/local
+srcdir=$PWD
+
+echo "Building Ipopt from ${srcdir}"
+echo "Saving headers and libraries to ${prefix}"
+
+# BLAS
+cd $srcdir/ThirdParty/Blas
+./get.Blas
+mkdir -p build && cd build
+../configure --prefix=$prefix --disable-shared --with-pic
+make install
+
+# Lapack
+cd $srcdir/ThirdParty/Lapack
+./get.Lapack
+mkdir -p build && cd build
+../configure --prefix=$prefix --disable-shared --with-pic \
+    --with-blas="$prefix/lib/libcoinblas.a -lgfortran"
+make install
+
+# ASL
+cd $srcdir/ThirdParty/ASL
+./get.ASL
+
+# MUMPS
+cd $srcdir/ThirdParty/Mumps
+./get.Mumps
+
+# build everything
+cd $srcdir
+./configure --prefix=$prefix coin_skip_warn_cxxflags=yes \
+    --with-blas="$prefix/lib/libcoinblas.a -lgfortran" \
+    --with-lapack=$prefix/lib/libcoinlapack.a
+make
+make test
+make -j1 install