《无人驾驶车辆模型预测控制第一版》第五章代码（下）

 本代码基于无人驾驶车辆模型预测控制第一版本第五章中的代码予以纠正，下面代码已经过测试。

通过函数来生成参考轨迹（双移线），然后用模型预测控制器去跟踪。

需要会员课程的朋友可以点击文末小卡片

加入 HUIYUAN 全年无限制学习后台（MPC相关矩阵的底层逻辑、纵向控制、非线性系统线性化处理及MPC算法动力学跟踪任何轨迹等）专属爆品课程（赠送会员专属全套答疑课程及全套爆品资源，且课程持续更新）！

代码段：

function [sys,x0,str,ts] = chapter5_2_2(t,x,u,flag)

switch flag,
 case 0
  [sys,x0,str,ts] = mdlInitializeSizes; % Initialization
  
 case 2
  sys = mdlUpdates(t,x,u); % Update discrete states
  
 case 3
  sys = mdlOutputs(t,x,u); % Calculate outputs
 
%  case 4
%   sys = mdlGetTimeOfNextVarHit(t,x,u); % Get next sample time 

 case {1,4,9} % Unused flags
  sys = [];
  
 otherwise
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%==============================================================
% Initialization
%==============================================================

function [sys,x0,str,ts] = mdlInitializeSizes

% Call simsizes for a sizes structure, fill it in, and convert it 
% to a sizes array.

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 6;
sizes.NumOutputs     = 1;
sizes.NumInputs      = 8;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes); 
x0 =[0.001;0.0001;0.0001;0.00001;0.00001;0.00001];    
global U;%U为我们的控制量
U=[0];%控制量初始化,这里面加了一个期望轨迹的输出，如果去掉，U为一维的

% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
 ts  = [0.05 0];       % sample time: [period, offset]，采样时间影响最大。
% ts  = [0.1 0];       % sample time: [period, offset]，采样时间影响最大。
%End of mdlInitializeSizes
		      
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
  
sys = x;
%End of mdlUpdate.

%=