%% 清屏
clear ;
close all;
clc;
%% 第一步，定义状态空间矩阵
%% 定义状态矩阵 A, n x n 矩阵
A = [1-(1e-5)*0.05/(0.002),100*pi*(1e-5);-100*pi*(1e-5),1-(1e-5)*0.05/(0.002)];
n= size (A,1);
%% 定义输入矩阵 B, n x p 矩阵
B = [(1e-5)/(0.002),0;0,(1e-5)/(0.002)];
p = size(B,2);
%% 定义Q矩阵，n x n 矩阵
Q=[400 0;0 700];
%% 定义F矩阵，n x n 矩阵
F=[1 0;0 1];
%% 定义R矩阵，p x p 矩阵
R=[1 0;0 1];
%% 定义step数量k
k_steps=100;
%% 定义预测区间K
N=5;
%% 定义矩阵 i_K， n x k 矩 阵
i_K = zeros(n,k_steps);
i_K(:,1) =[0;0];
%% 初始状态变量值， n x 1 向量
%iref = zeros((N+1)*2,1);
i_ref = -100;
iref = [i_ref;0;i_ref;0;i_ref;0;i_ref;0;i_ref;0;i_ref;0];
%% 定义输入矩阵 U_K， p x k 矩阵
%%V_K = zeros(p,k_steps);
%%e = [536.6;9.12];
U_K = zeros(p,k_steps);
%%U_K(:,1) = V_K(:,1) - e;
%% Call MPC_Matrices 函数 求得 E,H矩阵
[E,H,Z]=MPC_Matrices(A,B,Q,R,F,N);
%% 计算每一步的状态变量的值
for k = 1 : k_steps
%% 求得U_K(:,k)
U_K(:,k) = Prediction(i_K(:,k),iref,E,H,N,Z,p);
%% 计算第k+1步时状态变量的值
i_K(:,k+1)=(A*i_K(:,k)+B*U_K(:,k));
end
%% 绘制状态变量和输入的变化
subplot (2, 1, 1);
hold;
for i =1 :size (i_K,1)
plot (i_K(i,:));
end
legend("x1","x2")
hold off;
subplot (2, 1, 2);
hold;
for i =1 : size (U_K,1)
plot (U_K(i,:));
end
legend("u1","u2")
