%% No Change below
% Inertia Definition
Jm =0.00007124; %电机转动惯量 （kg*m^2）
Jfw = 0.0012; %转向轮绕主销的转动惯量（kg*m^2）
Mr =32; %减速机构、小齿轮和齿条等的当量质量（KG）
% Resistance coefficient
bm = 0.03339;%0.003339; %助力电机粘性阻尼系数 (N*m*rad^(-1)*s)
bkp = 0.04; %转向轮绕主销粘性阻尼系数(N*m*rad^(-1)*s)
br = 65.3; %齿条粘性阻尼系数（N*S）
% Elastic coefficient
Km = 12.5; %电机轴刚度（N*m*rad^(-1)）
Kkp = 10.0; %主销刚度
Kc =115; %115; %转向轴刚度(N/m)%（N*m*rad^(-1)）
% Mech Param
rp = 7.783; %小齿轮半径 （mm）
Nl = 16; %齿条到转向前轮的传动比
Ratio = 19; %减速构减速比
A = [Jm 0 0; 0 Jfw 0; 0 0 Mr];
Inv_A = inv(A);
B = [bm 0 0; 0 bkp 0; 0 0 br];
C = [Km, 0, -Km/rp;0, Kkp, (-Kkp/Nl);(-Km*Ratio/rp), (-Kkp), (Km*Ratio^2/rp^2+Kc/rp^2+Kkp/Nl^2)];
%C = [Km, 0, -Km/rp;0, Kkp, (Kkp/Nl);(-Km*Ratio/rp), (-Kkp), (Km*Ratio^2/rp^2+Kc/rp^2-Kkp/Nl^2)];
D = [0 1 0; 0 0 1; Kc/rp 0 0];
% Vehicle Param
Dx = 0.6; %轮胎的拖距(m)
m = 1400; % 整车质量(kg)
%v = 30; % 车速(m/s)
Iz= 1309.8; %绕重心Z轴转动惯量（Kg*m^2）
K1 = 636; %-70847； 前轮综合侧偏刚度 N/degree.Vehicle mass 1400kg, Front Wheels' Z-Load = 1400/4*2*10 = 7000N. Look up table get k estimate value 800N/degree.
K2 = 1201;%-71120; 后轮综合侧偏刚度 N/degree
a = 1.168; %质心至前轴的水平距离(m)
b = 1.328; %质心至后轴的水平距离(m)
Ch = 1; Kh = 126;
Jv=10000; %整车绕质心的转动惯量b
% ReturnTrq_Load = [1000, 2000, 3000, 4000, 5000, 6000];
% ReturnTrq_Beta = [0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20];
% ReturnTrq_Trq = [];
ThetaC = readtable('0824.xlsx');
