clear
load C K
load M.mat
f = linspace(0, 40, 100);
w = 2*pi*f;
F = @(t) [sin(2*pi*f(1)*t); zeros(3, 1)];
x0 = zeros(4, 1);
v0 = zeros(4, 1);
y0 = [x0; v0];
ind = 100;
tspan = [0 20];
mag = zeros(size(w));
phase = zeros(size(w));
X = zeros(length(w), 4);
for i = 1:length(w)
F = @(t) [sin(w(i)*t); zeros(3, 1)];
[~, y] = ode45(@(t, y) vibration_equation(t, y, M, C, K, F), tspan, y0);
x = y(end, 1:4)';
mag(i) = abs(x(1));
phase(i) = angle(x(1)) * 180/pi;
X(i,:) = x';
mpp = max(mag(1,:))/30;
[Ypk,Xpk,Wpk,Ppk] = findpeaks(mag(1,:),'MinPeakProminence',mpp, 'WidthReference','halfheight');
Xpk = Xpk + min(size(mag)) - 1;
[~,mxidx] = maxk(Ppk,4);
Ypkc{i} = Ypk(mxidx);
Xpkc{i} = Xpk(mxidx);
Wpkc{i} = Wpk(mxidx);
Ppkc{i} = Ppk(mxidx);
freq{i} = f(Xpkc{i});
end
figure;
subplot(2, 1, 1);
plot(f,mag,f(Xpk),Ypk,'dr');
xlabel('Frequency (Hz)');
ylabel('Magnitude');
ylim([0 3.5/10000]);
title('Frequency Response Function');
grid on;
subplot(2, 1, 2);
plot(f, mod(phase+180,360)-180,f(Xpk),Ypk,'dr');
xlabel('Frequency (Hz)');
ylabel('Phase (deg)');
ylim([-180, 180]);
grid on;
Ypkc = cell(1,4);
Xpkc = cell(1,4);
Wpkc = cell(1,4);
Ppkc = cell(1,4);
for i = 1:4
mpp(1,i) = max(X(:,i))/40;
[Ypk,Xpk,Wpk,Ppk] = findpeaks(X(:,i), 'MinPeakProminence', mpp(1,i), 'WidthReference', 'halfheight');
Xpk = Xpk + 1;
[~, mxidx] = maxk(Ppk, 4);
Ypkc{i} = Ypk(mxidx);
Xpkc{i} = Xpk(mxidx);
Wpkc{i} = Wpk(mxidx);
Ppkc{i} = Ppk(mxidx);
end
figure;
subplot(2, 2, 1);
plot(f, abs(X(:,1)), 'b');
hold on;
plot(f(Xpk), Ypk, 'dr');
xlabel('Frequency (Hz)');
ylabel('Displacement (m)');
ylim([0 3.5/10000]);
title('X1');
grid on;
subplot(2, 2, 2);
plot(f, abs(X(:,2)),'b');
hold on;
plot(f(Xpk), Ypk, 'dr');
xlabel('Frequency (Hz)');
ylabel('Displacement (m)');
ylim([0 3.5/10000]);
title('X2');
grid on;
subplot(2, 2, 3);
plot(f, abs(X(:,3)),'b');
hold on;
plot(f(Xpk), Ypk, 'dr');
xlabel('Frequency (Hz)');
ylabel('Displacement (m)');
ylim([0 3.5/10000]);
title('X3');
grid on;
subplot(2, 2, 4);
plot(f, abs(X(:,4)),'b');
hold on;
plot(f(Xpk), Ypk, 'dr');
xlabel('Frequency (Hz)');
ylabel('Displacement (m)');
ylim([0 3.5/10000]);
title('X4');
grid on;
clear C damp_ratio F i ind K K1_Th K2_Th M Natural_freq tspan mxidx
function dydt = vibration_equation(t, y, M, C, K, F)
x = y(1:4);
v = y(5:8);
a = M \ (F(t) - C*v - K*x);
dydt = [v; a];
end