function [u,MPC_case] = MPC_calculation_delta_u(x0,yref_Np,MPC_case)
% MPC Set-up
nx = MPC_case.nx;
ny = MPC_case.ny;
nu = MPC_case.nu;
xref_vec = MPC_case.C_pinv*yref_Np;
xref_vec = [xref_vec; zeros(nu,MPC_case.Np)];
xref_vec = reshape(xref_vec,(nx+nu)*MPC_case.Np,1);
if ~isfield(MPC_case,'opt') % This condition is true only the first time this function is called
% Define A_tilde, B_tilde, C_tilde for delta_u formulation
MPC_case.A_tilde = [MPC_case.A, MPC_case.B; zeros(nu,nx), eye(nu)];
MPC_case.B_tilde = [MPC_case.B; eye(nu)];
MPC_case.C_tilde = [MPC_case.C, zeros(ny,nu)];
% Compute mapping from present (initial) state, Ahat, and future inputs, Bhat,
% to future states: x_future=Ahat*x0+Bhat*u_future
Ahat = zeros(MPC_case.Np*(nx+nu),nx+nu);
Bhat = zeros(MPC_case.Np*(nx+nu),MPC_case.Np*nu);
for i = 1:MPC_case.Np
idx_i = ((i-1)*(nx+nu)+1):i*(nx+nu);
Ahat(idx_i,:) = MPC_case.A_tilde^i;
for j = 1:MPC_case.Np
idx_j = ((j-1)*nu+1):j*nu;
if i >= j
Bhat(idx_i,idx_j) = MPC_case.A_tilde^(i-j)*MPC_case.B_tilde;
end
end
end
MPC_case.opt.Ahat = Ahat;
MPC_case.opt.Bhat = Bhat;
% Get system and MPC data
MPC_case.Qx = MPC_case.C'*MPC_case.Qy*MPC_case.C;
MPC_case.S = MPC_case.C'*MPC_case.Sy*MPC_case.C;
% Stack up state and output weights in block-diagonal matrices to
% replace summing in objective function by matrix multiplication in QP
Qxhat = zeros(MPC_case.Np*(nx+nu),MPC_case.Np*(nx+nu));
for i = 1:MPC_case.Np
idx = (i-1)*(nx+nu)+1:(i-1)*(nx+nu)+nx;
if i < MPC_case.Np
Qxhat(idx,idx) = MPC_case.Qx;
else
Qxhat(idx,idx) = MPC_case.S;
end
end
% Qxhat = sparse(Qxhat);
MPC_case.opt.Qxhat = Qxhat;
Quhat = zeros(nu*MPC_case.Np,nu*MPC_case.Np);
for i = 1:MPC_case.Np
idx = (i-1)*nu+1:i*nu;
Quhat(idx,idx) = MPC_case.Qu;
end
% Quhat = sparse(Quhat);
% Constraints on states in matrix form: y_min<Hhat*x<y_max
Hhat = zeros(MPC_case.Np*ny,MPC_case.Np*(nx+nu));
for i = 1:MPC_case.Np
idx_i = (i-1)*ny+1:i*ny;
idx_j = (i-1)*(nx+nu)+1:i*(nx+nu);
Hhat(idx_i,idx_j) = MPC_case.C_tilde;
end
% Hhat = sparse(Hhat);
MPC_case.opt.Hhat = Hhat;
%% Sets up objective function and constraints for QP problem
% Constraints
Aineq_ymax = Hhat*Bhat;
Aineq_ymin = -Aineq_ymax;
Aineq_xmax = Bhat;
Aineq_xmin = -Aineq_xmax;
MPC_case.opt.Aineq = [Aineq_ymax; Aineq_ymin; Aineq_xmax; Aineq_xmin];
% Lower & Upper bounds for variables
MPC_case.opt.delta_u_max = repmat(MPC_case.delta_u_max,MPC_case.Np,1);
MPC_case.opt.delta_u_min = repmat(MPC_case.delta_u_min,MPC_case.Np,1);
% Obj func
H = Bhat'*Qxhat*Bhat+Quhat;
% H = (H+H')/2;
round_digit = 9;
MPC_case.opt.H = round(H*10^round_digit)/10^round_digit; % Added to avoid CPLEX error about H being not symmetric
end
Xdev = MPC_case.opt.Ahat*x0-xref_vec;
bineq_ymax = repmat(MPC_case.ymax,MPC_case.Np,1)-MPC_case.opt.Hhat*(Xdev+xref_vec);
bineq_ymin = repmat(-MPC_case.ymin,MPC_case.Np,1)+MPC_case.opt.Hhat*(Xdev+xref_vec);
bineq_xmax = repmat(MPC_case.xmax,MPC_case.Np,1)-(Xdev+xref_vec);
bineq_xmin = repmat(-MPC_case.xmin,MPC_case.Np,1)+(Xdev+xref_vec);
bineq = [bineq_ymax; bineq_ymin; bineq_xmax; bineq_xmin];
f = Xdev'*MPC_case.opt.Qxhat*MPC_case.opt.Bhat;
% Calculate future Np inputs:
[delta_u,~,exitflag] = quadprog(MPC_case.opt.H,f,MPC_case.opt.Aineq,bineq,[],[],MPC_case.opt.delta_u_min,MPC_case.opt.delta_u_max);
x1 = MPC_case.A_tilde*x0+MPC_case.B_tilde*delta_u(1:nu);
u = x1(nx+1:nx+nu);
% cplexqp can also be used here with the same inputs and outputs if it is
% installed.
if exitflag<0
disp('WARNING: infeasible QP problem.')
pause(0.1)
end
没有合适的资源?快使用搜索试试~ 我知道了~
资源推荐
资源详情
资源评论
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![7z](https://img-home.csdnimg.cn/images/20210720083312.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![7z](https://img-home.csdnimg.cn/images/20210720083312.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
收起资源包目录
![package](https://csdnimg.cn/release/downloadcmsfe/public/img/package.f3fc750b.png)
![folder](https://csdnimg.cn/release/downloadcmsfe/public/img/folder.005fa2e5.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![folder](https://csdnimg.cn/release/downloadcmsfe/public/img/folder.005fa2e5.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
![file-type](https://csdnimg.cn/release/download/static_files/pc/images/minetype/UNKNOWN.png)
共 9 条
- 1
资源评论
![avatar-default](https://csdnimg.cn/release/downloadcmsfe/public/img/lazyLogo2.1882d7f4.png)
![avatar](https://profile-avatar.csdnimg.cn/d5fa1452106248a4a63014172db25c5d_leavemyleave.jpg!1)
mYlEaVeiSmVp
- 粉丝: 1961
- 资源: 19万+
![benefits](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-1.c8e153b4.png)
下载权益
![privilege](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-2.ec46750a.png)
C知道特权
![article](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-3.fc5e5fb6.png)
VIP文章
![course-privilege](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-4.320a6894.png)
课程特权
![rights](https://csdnimg.cn/release/downloadcmsfe/public/img/vip-rights-icon.fe0226a8.png)
开通VIP
上传资源 快速赚钱
我的内容管理 展开
我的资源 快来上传第一个资源
我的收益
登录查看自己的收益我的积分 登录查看自己的积分
我的C币 登录后查看C币余额
我的收藏
我的下载
下载帮助
![voice](https://csdnimg.cn/release/downloadcmsfe/public/img/voice.245cc511.png)
![center-task](https://csdnimg.cn/release/downloadcmsfe/public/img/center-task.c2eda91a.png)
安全验证
文档复制为VIP权益,开通VIP直接复制
![dialog-icon](https://csdnimg.cn/release/downloadcmsfe/public/img/green-success.6a4acb44.png)