1. %% Question 7
2. %==========================================================================
3. % Setup MPC controller
4. %==========================================================================
5.  
6. %sp=zeros(tf,3);         % setpoint trajectory
7.  
8. N=10;                   % prediction horizon
9. M=3;                    % control horizon
10.  
11. Q = diag([1 0.001 1]);  % state penalty
12. Pf = Q;                 % terminal state penalty
13. R = 0.01*eye(m);         % control penalty
14.  
15. x0=[0.01;1;0.1];
16.  
17. tf_vec = 0:1:tf;
18.  
19. p_vec = ones(nd,tf).*d';
20.  
21. XsUs = zeros(n+m,tf);
22.  
23. xe_est = zeros(n+nd,tf);
24. x_ = zeros(n,tf);
25. d_ = zeros(nd,tf);
26.  
27. xs = zeros(n,tf);
28. us = zeros(m,tf);
29.  
30. xd = zeros(n,tf);
31. ud = zeros(m,tf);
32.  
33. x = zeros(n,tf);
34. x(:,1) = x0;
35.  
36. u = zeros(m,tf);
37. y = zeros(p,tf);
38.  
39.  
40. %==========================================================================
41. % Simulation
42. %==========================================================================
43.    
44. % Simulation
45.    
46.     for k = 1:tf
47.        
48.         y(:,k) = C*x(:,k) + Cd*p_vec(:,k);
49.         %=============================
50.         % Calculate steady state target
51.         %=============================
52.         XsUs(:,k) = Mss*d_(:,k);
53.        
54.         xs(:,k) = XsUs(1:n,k);
55.         us(:,k) = XsUs(n+1:end,k);
56.        
57.         %=============================
58.         % Solve the QP
59.         %=============================
60.         xd(:,k) = x_(:,k) - xs(:,k);
61.        
62.         [ud(:,k), ~] = RHC(A,B,Q,R,Pf,N,M,xd(:,k));
63.        
64.         u(:,k) = ud(:,k)+ us(:,k);
65.         %=============================
66.         % Update the observer state
67.         %=============================
68.         xe_est(:,k+1)= Ae*xe_est(:,k) + Be*u(:,k) + Le*(y(:,k)-Ce*xe_est(:,k));
69.        
70.         x_(:,k+1) = xe_est(1:n,k+1);
71.         d_(:,k+1) = xe_est(n+1:end,k+1);
72.        
73.         %=============================
74.         % Update the process state
75.         %=============================
76.        
77.         x(:,k+1) = A*x(:,k) + B*u(:,k) + Bd*p_vec(:,k);
78.        
79.         %=============================        
80.         % Store current variables in log
81.         %=============================
82.  
83.    end % simulation loop