clc
clear all
close all
syms v r psi delta_r
PSV1 = [v; r; psi];
PA_A1 = [-0.736243229602963,-0.593872968776296,0;0.105400050677545,-0.887577743421107,0;0,1,0];
PA_B1 = [0.168078575433693;-0.565331828240409;0];
Input_Data_2019_06_13 = readmatrix('actuators_node.fins.dat');
Input_Data_2019_06_13(:,1) = Input_Data_2019_06_13(:,1) - Input_Data_2019_06_13(1,1) ;
Rudder_Angle = (-Input_Data_2019_06_13(:,4) + Input_Data_2019_06_13(:,5))/2;
Rudder_Angle = Rudder_Angle(768:2616)*(pi/180);
U = Rudder_Angle;
U(isnan(U)) = 0;
t_old = transpose(linspace(0,100,length(Input_Data_2019_06_13(768:2616,:))));
t_new = transpose(linspace(0,100,length(Input_Data_2019_06_13(768:2616,:)))) ;
U_new = interp1(t_old,U,t_new);
U_new(isnan(U_new)) = 0;
dv = diff(v);
dr = diff(r);
dpsi = diff(psi);
eqn1 = dv == PA_A1(1,1)*v + PA_A1(1,2)*r + PA_A1(1,3)*psi + PA_B1(1,1) * U_new;
eqn2 = dr == PA_A1(2,1)*v + PA_A1(2,2)*r + PA_A1(2,3)*psi + PA_B1(2,1) * U_new;
eqn3 = dpsi == PA_A1(3,1)*v + PA_A1(3,2)*r + PA_A1(3,3)*psi + PA_B1(3,1) * U_new;
PSV1_0(1) = 0;
PSV1_0(2) = 0;
PSV1_0(3) = 0;
[eqs, vars] = reduceDifferentialOrder([eqn1, eqn2, eqn3], [v, r, psi]);
[M,F] = massMatrixForm(eqs,vars);
f = M\F;
odefun = odeFunction(f,vars);
disp(odefun)
[t,y] = ode45(odefun, t_new, PSV1_0);
figure
subplot(2,2,1)
plot(t_new,y(:,1),'-r');
grid on;
ylabel('v(t) in m/s');
xlabel('t in sec');
subplot(2,2,2)
plot(t_new,y(:,2),'-r');
grid on;
ylabel('r(t) in m/s');
xlabel('t in sec');
subplot(2,2,3)
plot(t_new,y(:,3),'-r');
grid on;
ylabel('psi(t) in m/s');
xlabel('t in sec');