close all

%% DATA
m_s=3500;
Ixx=1900;
Iyy=6500;
a=1.78;
b=1.32;
l=0.64;
m_f=45;     % m_fr=m_fl=75
m_r=50;     % m_rr=m_rl=50
m_b=120;

ks_fH=187000;     % ks_fr=ks_fl=198000/2
ks_rH=167000;     % ks_rr=ks_rl=198000/2
cs_fH=2200;    % cs_fl=cs_fr=12500/2
cs_rH=2600;    % cs_rr=cs_rl=12500/2
ku=370000;    % ku_fr=ku_fl=ku_rr=ku_rl=ku

%% Point 1
% xb=(2270-1415)/1000;
% yb=0.43;

%% Point 2
xb=(667+2270-1415)/1000;
yb=0.43;

%% keep c_b constant, varying k_b, finding the optimum k_b. 

k_b1=2000;

M_HIS=[m_s 0   0   0   0   0   0;
       0   Iyy 0   0   0   0   0;
       0   0   Ixx 0   0   0   0;
       0   0   0   m_f 0   0   0;
       0   0   0   0   m_f 0   0;
       0   0   0   0   0   m_r 0;
       0   0   0   0   0   0   m_r];

K1=[k_b1     -k_b1                     -k_b1*xb                        k_b1*yb                         0       0       0       0;
   -k_b1    k_b1+ks_f*2+ks_r*2        k_b1*xb-ks_f*a*2+b*ks_r*2       -k_b1*yb                        -ks_f   -ks_f   -ks_r   -ks_r;
   -k_b1*xb k_b1*xb-2*ks_f*a+2*ks_r*b k_b1*xb^2+2*ks_f*a^2+2*ks_r*b^2 -k_b1*xb*yb                     ks_f*a  ks_f*a  -ks_r*b -ks_r*b;
   k_b1*yb  -k_b1*yb                 -k_b1*xb*yb                      k_b1*yb^2+2*ks_f*l^2+2*ks_r*l^2 -ks_f*l ks_f*l  ks_r*l  -ks_r*l;
   0       -ks_f                    ks_f*a                         -ks_f*l                        ks_f+ku 0       0       0;
   0       -ks_f                    ks_f*a                         ks_f*l                         0       ks_f+ku 0       0;
   0       -ks_r                    -ks_r*b                        ks_r*l                         0       0       ks_r+ku 0;
   0       -ks_r                    -ks_r*b                        -ks_r*l                        0       0       0       ks_r+ku];


%% keep k_b its optimum value, varying c_b, finding the optimum c_b. 

c_b1=250;

% M=[m_b 0   0   0   0   0   0   0;
%    0   m_s 0   0   0   0   0   0;
%    0   0   Iyy 0   0   0   0   0;
%    0   0   0   Ixx 0   0   0   0;
%    0   0   0   0   m_f 0   0   0;
%    0   0   0   0   0   m_f 0   0;
%    0   0   0   0   0   0   m_r 0;
%    0   0   0   0   0   0   0   m_r];

C1=[c_b1     -c_b1                     -c_b1*xb                        c_b1*yb                         0       0       0       0;
   -c_b1    c_b1+cs_f*2+cs_r*2        c_b1*xb-cs_f*a*2+b*cs_r*2       -c_b1*yb                        -cs_f   -cs_f   -cs_r   -cs_r;
   -c_b1*xb c_b1*xb-2*cs_f*a+2*cs_r*b c_b1*xb^2+2*cs_f*a^2+2*cs_r*b^2 -c_b1*xb*yb                     cs_f*a  cs_f*a  -cs_r*b -cs_r*b;
   c_b1*yb  -c_b1*yb                  -c_b1*xb*yb                      c_b1*yb^2+2*cs_f*l^2+2*cs_r*l^2 -cs_f*l cs_f*l  cs_r*l  -cs_r*l;
   0       -cs_f                    cs_f*a                         -cs_f*l                        cs_f    0    0    0;
   0       -cs_f                    cs_f*a                         cs_f*l                         0       cs_f 0    0;
   0       -cs_r                    -cs_r*b                        cs_r*l                         0       0    cs_r 0;
   0       -cs_r                    -cs_r*b                        -cs_r*l                        0       0    0    cs_r];

%% Parameters of sin signal

% w=2*pi/(6.5/(30/3.6));
% phi_front=-(2*pi/(6.5/(30/3.6)))*(6.5/4/(30/3.6));
% phi_rear=(2*pi/(6.5/(30/3.6)))*(3.25/(30/3.6))-(2*pi/(6.5/(30/3.6)))*(6.5/4/(30/3.6));


%% HIS suspension

% DATA
miu=1.6;
hr=1.6;
hf=0.625;
Cp=0;
Ca=3*10^(-10);
Srt=(0.05/2)^2*pi;
Z_A=3.5*10^8;
Z_H=4.7*10^8;
Z_R=3.8*10^8;
a=1.78;
b=1.32;
l=0.64;


% Stiffness Characteristic

Kb=[2*((miu-hr)^2+(hf*miu)^2)                        -2*((a*miu+b*hr)*(miu-hr)+(a*hf*miu+b)*(hf*miu)) 0;
    -2*((a*miu+b*hr)*(miu-hr)+(a*hf*miu+b)*(hf*miu)) 2*((a*miu+b*hr)^2+(a*hf*miu+b)^2)                0;
    0                                                0                                                2*((l*miu+l*hr)^2+(l*hf*miu+l)^2)];

Kbw=[-miu*(miu-hr)-hf*miu*hf*miu           -miu*(miu-hr)-hf*miu*hf*miu              hr*(miu-hr)+hf*miu            hr*(miu-hr)+hf*miu;
     miu*(a*miu+b*hr)+miu*hf*(a*hf*miu+b)  miu*(a*miu+b*hr)+miu*hf*(a*hf*miu+b)     -hf*(a*miu+b*hr)-(a*hf*miu+b) -hf*(a*miu+b*hr)-(a*hf*miu+b);
     -miu*(l*miu+l*hr)-hf*miu*(l*hf*miu+l) -(-miu*(l*miu+l*hr)-hf*miu*(l*hf*miu+l)) -hr*(l*miu+l*hr)-(l*miu*hf+l) -(-hr*(l*miu+l*hr)-(l*miu*hf+l))];

Kw=[miu^2*(hf^2+1) 0              0            -miu*(hr+hf);
    0              miu^2*(hf^2+1) -miu*(hr+hf) 0;
    0              -miu*(hr+hf)   hr^2+1       0;
    -miu*(hr+hf)   0              0            hr^2+1];

K_HIS=1/(Cp+Ca)*[Kb Kbw; Kbw' Kw;]*Srt^2;
K00=[0 0 0 0 0 0 0]';
K_HIS1=[1 0 0 0 0 0 0 0; K00 K_HIS];

% Damping Characteristic

i=0;

Cb_0=[2*(hf^i*miu^2+hr^i)      -2*(a*hf^i*miu^2-b*hr^i)    0;
      -2*(a*hf^i*miu^2-b*hr^i) 2*(a^2*hf^i*miu^2+b^2*hr^i) 0;
      0                        0                           2*(l^2*hf^i*miu^2+l^2*hr^i)];

Cbw_0=[-hf^i*miu^2   -hf^i*miu^2  -hr^i     -hr^i;
       a*hf^i*miu^2  a*hf^i*miu^2 -b*hr^i   -b*hr^i;
       -l*hf^i*miu^2 l*hf^i*miu^2 -l*b*hr^i l*b*hr^i];

Cw_0=[hf^i*miu^2 0          0    0;
      0          hf^i*miu^2 0    0;
      0          0          hr^i 0;
      0          0          0    hr^i];

j=2;

Cb_2=[2*(hf^j*miu^2+hr^j)      -2*(a*hf^j*miu^2-b*hr^j)    0;
      -2*(a*hf^j*miu^2-b*hr^j) 2*(a^2*hf^j*miu^2+b^2*hr^j) 0;
      0                        0                           2*(l^2*hf^j*miu^2+l^2*hr^j)];

Cbw_2=[-hf^j*miu^2   -hf^j*miu^2  -hr^j     -hr^j;
       a*hf^j*miu^2  a*hf^j*miu^2 -b*hr^j   -b*hr^j;
       -l*hf^j*miu^2 l*hf^j*miu^2 -l*b*hr^j l*b*hr^j];

Cw_2=[hf^j*miu^2 0          0    0;
      0          hf^j*miu^2 0    0;
      0          0          hr^j 0;
      0          0          0    hr^j];

Kco=[Kb Kbw; Kbw' Kw]*Srt^2;

C_HIS=Z_A*Kco+Z_H*[Cb_0 Cbw_0; Cbw_0' Cw_0]*Srt^2+Z_R*[Cb_2 Cbw_2; Cbw_2' Cw_2]*Srt^2;

C00=[0 0 0 0 0 0 0]';
C_HIS1=[0 0 0 0 0 0 0 0; C00 C_HIS];


%% Optimum kb and cb for random road
cb3=0;
kb=0;



    Kop1=[kb+ks_fH*2+ks_rH*2        kb*xb-ks_fH*a*2+b*ks_rH*2       -kb*yb                        -ks_fH   -ks_fH   -ks_rH   -ks_rH;
         kb*xb-2*ks_fH*a+2*ks_rH*b kb*xb^2+2*ks_fH*a^2+2*ks_rH*b^2 -kb*xb*yb                     ks_fH*a  ks_fH*a  -ks_rH*b -ks_rH*b;
         -kb*yb                 -kb*xb*yb                      kb*yb^2+2*ks_fH*l^2+2*ks_rH*l^2 -ks_fH*l ks_fH*l  ks_rH*l  -ks_rH*l;
         -ks_fH                    ks_fH*a                         -ks_fH*l                        ks_fH+ku 0       0       0;
         -ks_fH                    ks_fH*a                         ks_fH*l                         0       ks_fH+ku 0       0;
         -ks_rH                    -ks_rH*b                        ks_rH*l                         0       0       ks_rH+ku 0;
         -ks_rH                   -ks_rH*b                        -ks_rH*l                        0       0       0       ks_rH+ku];


    Cop1=[cb3+cs_fH*2+cs_rH*2        cb3*xb-cs_fH*a*2+b*cs_rH*2       -cb3*yb                        -cs_fH   -cs_fH   -cs_rH   -cs_rH;
   cb3*xb-2*cs_fH*a+2*cs_rH*b cb3*xb^2+2*cs_fH*a^2+2*cs_rH*b^2 -cb3*xb*yb                     cs_fH*a  cs_fH*a  -cs_rH*b -cs_rH*b;
   -cb3*yb                 -cb3*xb*yb                      cb3*yb^2+2*cs_fH*l^2+2*cs_rH*l^2 -cs_fH*l cs_fH*l  cs_rH*l  -cs_rH*l;
       -cs_fH                    cs_fH*a                         -cs_fH*l                        cs_fH    0    0    0;
         -cs_fH                    cs_fH*a                         cs_fH*l                         0       cs_fH 0    0;
        -cs_rH                    -cs_rH*b                        cs_rH*l                         0       0    cs_rH 0;
       -cs_rH                    -cs_rH*b                        -cs_rH*l                        0       0    0    cs_rH];


%%

% kb=2000;
% for cb3=50:20:1000
% 
%     Kv=[kb     -kb                     -kb*xb                        kb*yb                         0       0       0       0;
%    -kb    kb+ks_f*2+ks_r*2        kb*xb-ks_f*a*2+b*ks_r*2       -kb*yb                        -ks_f   -ks_f   -ks_r   -ks_r;
%    -kb*xb kb*xb-2*ks_f*a+2*ks_r*b kb*xb^2+2*ks_f*a^2+2*ks_r*b^2 -kb*xb*yb                     ks_f*a  ks_f*a  -ks_r*b -ks_r*b;
%    kb*yb  -kb*yb                 -kb*xb*yb                      kb*yb^2+2*ks_f*l^2+2*ks_r*l^2 -ks_f*l ks_f*l  ks_r*l  -ks_r*l;
%    0       -ks_f                    ks_f*a                         -ks_f*l                        ks_f+ku 0       0       0;
%    0       -ks_f                    ks_f*a                         ks_f*l                         0       ks_f+ku 0       0;
%    0       -ks_r                    -ks_r*b                        ks_r*l                         0       0       ks_r+ku 0;
%    0       -ks_r                    -ks_r*b                        -ks_r*l                        0       0       0       ks_r+ku];
% 
%     Cv=[cb3     -cb3                     -cb3*xb                        cb3*yb                         0       0       0       0;
%    -cb3    cb3+cs_f*2+cs_r*2        cb3*xb-cs_f*a*2+b*cs_r*2       -cb3*yb                        -cs_f   -cs_f   -cs_r   -cs_r;
%    -cb3*xb cb3*xb-2*cs_f*a+2*cs_r*b cb3*xb^2+2*cs_f*a^2+2*cs_r*b^2 -cb3*xb*yb                     cs_f*a  cs_f*a  -cs_r*b -cs_r*b;
%    cb3*yb  -cb3*yb                 -cb3*xb*yb                      cb3*yb^2+2*cs_f*l^2+2*cs_r*l^2 -cs_f*l cs_f*l  cs_r*l  -cs_r*l;
%    0       -cs_f                    cs_f*a                         -cs_f*l                        cs_f    0    0    0;
%    0       -cs_f                    cs_f*a                         cs_f*l                         0       cs_f 0    0;
%    0       -cs_r                    -cs_r*b                        cs_r*l                         0       0    cs_r 0;
%    0       -cs_r                    -cs_r*b                        -cs_r*l                        0       0    0    cs_r];
% 
m = sim('grade_C_road_HIS.slx','SimulationMode','normal');

n1b = m.get('dZbdotdot_HIS');
n2b = m.get('dZbdotdot');
n1 = m.get('dZsdotdot_HIS');
n2 = m.get('dZsdotdot');
n3 = m.get('dpitchdotdot_HIS');
n4 = m.get('dpitchdotdot');
n5 = m.get('drolldotdot_HIS');
n6 = m.get('drolldotdot');

n1bb = m.get('Zb_HIS');
n2bb = m.get('Zb');
n7 = m.get('Zs_HIS');
n8 = m.get('Zs');
n9 = m.get('pitch_HIS');
n10 = m.get('pitch');
n11 = m.get('roll_HIS');
n12 = m.get('roll');

assignin('base','g',n1);
assignin('base','g',n2);
assignin('base','g',n3);
assignin('base','g',n4);
assignin('base','g',n5);
assignin('base','g',n6);
assignin('base','g',n7);
assignin('base','g',n8);
assignin('base','g',n9);
assignin('base','g',n10);
assignin('base','g',n11);
assignin('base','g',n12);

RMS1b = sqrt(mean(sum(n1.*n1))) %RMS value from time domain samples 
RMS2b = sqrt(mean(sum(n2.*n2))) %RMS value from time domain samples 
RMS1 = sqrt(mean(sum(n1.*n1))) %RMS value from time domain samples 
RMS2 = sqrt(mean(sum(n2.*n2))) %RMS value from time domain samples 
RMS3 = sqrt(mean(sum(n3.*n3))) %RMS value from time domain samples 
RMS4 = sqrt(mean(sum(n4.*n4))) %RMS value from time domain samples 
RMS5 = sqrt(mean(sum(n5.*n5))) %RMS value from time domain samples 
RMS6 = sqrt(mean(sum(n6.*n6))) %RMS value from time domain samples 
RMS1bb = sqrt(mean(sum(n1.*n1))) %RMS value from time domain samples 
RMS2bb = sqrt(mean(sum(n2.*n2))) %RMS value from time domain samples 
RMS7 = sqrt(mean(sum(n7.*n7))) %RMS value from time domain samples 
RMS8 = sqrt(mean(sum(n8.*n8))) %RMS value from time domain samples 
RMS9 = sqrt(mean(sum(n9.*n9))) %RMS value from time domain samples 
RMS10 = sqrt(mean(sum(n10.*n10))) %RMS value from time domain samples 
RMS11 = sqrt(mean(sum(n11.*n11))) %RMS value from time domain samples 
RMS12 = sqrt(mean(sum(n12.*n12))) %RMS value from time domain samples 

Q=[RMS1b RMS2b; RMS1 RMS2; RMS3 RMS4; RMS5 RMS6; RMS1bb RMS2bb; RMS7 RMS8; RMS9 RMS10; RMS11 RMS12];
bar(Q);
xticks(1:8);
xticklabels({'dZb/dt/dt','dZs/dt/dt','dPitch/dt/dt','dRoll/dt/dt','Zb','Zs','Pitch','Roll'})
ylabel('RMS Value');
legend({'With HIS','Without HIS'})

% 
% hold on
% plot(cb3,RMS2,'*',cb3,RMS1,'.')
% grid
% hold off
% end
% 