Line search and LMS parameter adaptation

[markcannon]

Incorporated line search to recover from potential infeasibility due to linearising around a nominal trajectory. Line search implemented on control perturbation and initial state. Incorporated least mean square (LMS) parameter adaptation.

Line search iteration summary:
While iteration_flag == 1

1. If iteration_count = 1: update c_0 and c_00 by time-shifting 1 step and adding 0 at the end, set x0_0 equal to x_{t} (the current plant state), set x0_00 equal to the second element of x_0 (the previously computed nominal state sequence), set z_0 = (x0_0, c_0) and z_00 = (x0_00, c_00), and update Jold by subtracting l(x_{t-1},u_{t-1}) - beta.
2. Compute the nominal trajectory (x_0, u_0) using c_0 and the undisturbed nonlinear dynamics with initial condition x0_0, and compute the model convexification by linearising the concave parts about (x_0, u_0).
3. Perform the convex MPC optimisation (including the constraint J <= Jold) of c such that the predicted control sequence is u = K*(x-x_r) + c_0 + c + u_r, where x_r and u_r are state and input reference sequences.
4. If the MPC optimisation in step 3 is infeasible: do a line search step: z_0 = z_00 + alpha*(z_0 - z_00), extract x0_0 and c_0 such that (x0_0, c_0) = z_0, increment line_search_iteration_count and set iteration_flag = 0 if line_search_iteration_count > max_line_search_iterations.
5. Else: increment iteration_count and
   (a) if iteration_count > max_iterations or abs(J-Jold) < delta_J_tolerance or norm(c) < delta_u_tolerance: set iteration_flag = 0 and set iteration_count = 1;
   (b) set Jold = J, c_00 = c_0, and c_0 = c_0 + c.
6. Return to 1.