The model used to represent the state of vehicle at time t was a kinematic model which models an x and y coordinate relative to some (0, 0) origin as well as psi, the heading angle and velocity vector v. As with many control algorithms,a cost function is introducing in order to be minimized. The two major errors tracked are cross track error and psi error.
The actuators used are accelaration between 1.0 and -1.0, where negative accelartion corresponds to breaking, and steering angle with range [-25 degrees, 25 degrees].
Below are the formulas generated when all the variables come together to create the state {xt+1, yt+1, ψt+1, vt+1, ctet+1, eψt+1} below:
The values selected were based on those used in the guided study for the project. Originally I had thought to use n = 25 and dt = 0.05 but upon seeing the values from the guided study I thought about to change my chosen N and dt. The primary reason being that my values could potentially hamper the performance of the MPC especially considering a 100ms latency.
The preprocessing for my MPC implementation consisted of transforming the state as it was received from a map-based orientation to a vehicle-based orientation. This allowed the calculations for coefficients, cte, and the psi error.
To combat the latency problem as directly as possible I have a check in MPC.cpp to check for values of T larger than 1. In this event it means we're "behind" so to speak. Instead of using values that may put the car's eventual trajectory over the yellow line and cause oscillation, we can simply use the previous actuator values at t - 2 and eliminate the oscillation. The other way to help with the negative results of latency on the path of the car is to add cost to the car when it's having to use actuators often and with large values between sequential actions. Minimizing this cost will allow the potential effects of latency at higher speeds and around corners to be handled safely.