MPC路径跟踪

function [sysx0strts] = chapter5_2_2（txuflag）
% 该程序功能：用LTV MPC 和车辆简化动力学模型（小角度假设）设计控制器，作为Simulink的控制器
% 程序版本 V1.0，MATLAB版本：R2011a采用S函数的标准形式，
% 程序编写日期 2013.12.11
% 最近一次改写 2013.12.16
% 状态量=[y_dotx_dotphiphi_dotYX]，控制量为前轮偏角delta_f


switch flag
 case 0
  [sysx0strts] = mdlInitializeSizes; % Initialization
  
 case 2
  sys = mdlUpdates（txu）; % Update discrete states
  
 case 3
  sys = mdlOutputs（txu）; % Calculate outputs
 
%  case 4
%   sys = mdlGetTimeOfNextVarHit（txu）; % Get next sample time 

 case {149} % Unused flags
  sys = [];
  
 otherwise
  error（[‘unhandled flag = ‘num2str（flag）]）; % Error handling
end
% End of dsfunc.

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

function [sysx0strts] = 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=[0];%控制量初始化这里面加了一个期望轨迹的输出，如果去掉，U为一维的
% global x;
% x = zeros（md.ne + md.pye + md.me + md.Hu*md.me1）;   
% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.02 0];       % sample time: [period offset]
%End of mdlInitializeSizes
		      
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates（txu）
  
sys = x;
%End of mdlUpdate.

%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs（txu）
    global a b; 
    %global u_piao;
    global U;
    %global kesi;
    tic
    Nx=6;%状态量的个数
    Nu=1;%控制量的个数
    Ny=2;%输出量的个数
    Np =20;%预测步长
    Nc=5;%控制步长
    Row=1000;%松弛因子权重
    fprintf（‘Update start t=%6.3f\n‘t）
   
    %输入接口转换x_dot后面加一个非常小的数，是防止出现分母为零的情况
   % y_dot=u（1）/3.6-0.000000001*0.4786;%CarSim输出的是km/h，转换为m/s
    y_dot=u（1）/3.6;
    x_dot=u（2）/3.6+0.0001;%CarSim输出的是km/h，转换为m/s
    phi=u（3）*3.141592654/180; %CarSim输出的为角度，角度转换为弧度
    phi_dot=u（4）*3.141592654/180;
    Y=u（5）;%单位为m
    X=u（6）;%单位为米
    Y_dot=u（7）;
    X_dot=u（8）;
%% 车辆参数输入
%syms sf sr;%分别为前后车轮的滑移率需要提供
    Sf=0.2; Sr=0.2;
%syms lf lr;%前后车轮距离车辆质心的距离，车辆固有参数
    lf=1.232;lr=1.468;
%syms C_cf C_cr C_lf C_lr;%分别为前后车轮的纵横向侧偏刚度，车辆固有参数
    Ccf=66900;Ccr=62700;Clf=66900;Clr=62700;
%syms m g I;%m为车辆质量，g为重力加速度，I为车辆绕Z轴的转动惯量，车辆固有参数
    m=1723;g=9.8;I=4175;
   

%% 参考轨迹生成
    shape=2.4;%参数名称，用于参考轨迹生成
    dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
    dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
    Xs1=27.19;Xs2=56.46;%