% LSIM and FFT example
% 
% This program demonstrates the state space use of lsim with a single-input, 
% multi-output system.  In addition to reading, running, and understanding this .m file,
% it is reccomended that you read the help files associated with lsim by typing 
% 'help lsim.'
%
% Matthew Schwall  May 2002

clear all;  %Clear all variables 
close all;  %Close all open figure windows 

% This tutorial will use the simple quarter-car Model with the sign conventions and variable 
% names described in Chris Gerdes' lecture notes.

% The constants we will use are:
ks=17000; %[N/m]
kt=150000; %[N/m]
M=360; %[kg]
m=40; %[kg]
Cs=1000; %[N/m/s]

% The first step is to transform the equations of motion into state space form:
%          .
%          x = A x + B u 
%                         
%          y = C x + D u 
% 
% We choose our state variables to be the position and velocities of the two masses.  
% Therefore, x = [Z Zdot Zu Zudot] where Z and Zu are the positions of the sprung and 
% unsprung masses respectively.
% 
% The input to the system is Zr, the position of the road, and the outputs are the position 
% and acceleration of the sprung mass.
% 
% We put the system in state space form by rearranging our equations of motion such that:
% 
% Z' = Zdot
% Zdot' = ks*Zu/M - ks*Z/M + Cs*Zudot/M - Cs*Zdot/M
% Zu' = Zudot
% Zudot' = ks*Z/m - ks*Zu/m + Cs*Zdot/m - Cs*Zudot/m + kt*Zr/m - kt*Zu/m
% 
% so that 

A = [ 0       1         0        0
    -ks/M  -Cs/M      ks/M     Cs/M
    0         0         0        1
    ks/m    Cs/m  -(ks+kt)/m  -Cs/m];

B = [0
    0
    0
    kt/m];

C = [ 1       0         0        0
    -ks/M  -Cs/M      ks/M     Cs/M];

D = [0
     0];

sys = ss(A,B,C,D); % This defines the system from our state space matrices
 
% In order to use lsim, we need both a time vector and an input vector.  
% For this example, our input will be a chirp sampled at 200hz for 10 seconds.

Ts=200; % Our sampling frequency
t = 0:1/Ts:10;  % The time vector
Zr = .05*chirp(t,0,10,30);  %The road displacement is a (very unrealistic) chirp

% Note that if our system had more than one input, the input vector would 
% have as many columns as inputs.

% While it is optional, one can also specify the initial conditions for each 
% of the states.  

X0 = [0 0 0 0]'; % The system is initially at rest

% In order to simulate the output from this input, we call lsim

[Y,T,X] = lsim(sys, Zr, t, X0); %Y is the output, T is the time, and X 
                                %the time history of our states

Z = Y(:,1);  %Sprung mass position was our first output
Zacc = Y(:,2);  %Sprung mass acceleration was our second output

plot(t, Zr, 'k-', t, Z, 'b-');
title('Response to swept sinusoid input')
xlabel('Time (s)')
ylabel('Displacement (m)')
legend('Road position','Sprung mass position');

figure
plot(t, Zacc, 'b-');
title('Response to swept sinusoid input')
xlabel('Time (s)')
ylabel('Acceleration (m/s/s)')
legend('Sprung mass acceleration');