%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PVTOL
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear
syms P alfa V  teta x y g
% syms dV dteta dy dx 
syms x1_st y1_st
 
%Cистема диф. уравнений PVTOL
%Сигналы управления P alfa
% lambda = 1
dV     =  0.00286*(P - 0.5*((0.017+0.025*V/(340.192-0.004*y))+(1.188+0.356*V/(340.192-0.004*y))^2 * (alfa+0.007)^2)*1.715*exp(-0.0001*y)*V^2)-g*sin(teta);
dteta  =  0.00286/V*(P*(alfa+0.061)+0.5*(4.312+1.291*V/(340.192-0.004*y))*(alfa+0.007)*1.715*exp(-0.0001*y)*V^2)-g*cos(teta)/V;
dx     =  V*cos(teta);
dy     =  V*sin(teta);
 
% Записываем как систему уравнений (правые части)
Syst = [dV; dteta; dx; dy]; 
 
% Точка равновесия X0 и U0 для линеаризации
X0 = [x1_st; 0; y1_st; 0];
U0 = [g; 0];
 
% Подставляем в систему точку равновесия
Syst_st = subs(Syst,[V; teta; x; y; P; alfa],[X0; U0]);
 
% Дифференцируем систему по переменным x1 x2 y1 y2 phi1 phi2
JA=jacobian(Syst, [V teta x y]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear
syms P alfa V  teta x y g 
syms t % время
 
%Cистема диф. уравнений LA
%Сигналы управления P alfa
dV    = 0.00286*(P - 0.5*((0.017+0.025*V/(340.192-0.004*y)) + ...
         (1.188+0.356*V/(340.192-0.004*y))^2 * ...
         (alfa+0.007)^2)*1.715*exp(-0.0001*y)*V^2) - g*sin(teta);
dteta = 0.00286/V*(P*(alfa+0.061) + 0.5*(4.312+1.291*V/(340.192-0.004*y))* ...
         (alfa+0.007)*1.715*exp(-0.0001*y)*V^2) - g*cos(teta)/V;
dx    = V*cos(teta);
dy    = V*sin(teta);
 
% Записываем как систему уравнений (правые части)
Syst = [dV; dteta; dx; dy]; 
 
% Точка равновесия X01 и U01 для линеаризации
X01 = [97.5; 0; 1000+97.5*t; 2000];
U01 = [292.782; 0.101];
 
% Подставляем в систему точку равновесия X01 U01
Syst_st1 = subs(Syst,[V; teta; x; y; P; alfa; g],[X01; U01; 9.81]);
eval(Syst_st1)
%Первое значение вектора не равно нулю, возможно надо подобрать лучше
%значения входов 
%Третье значение 97.5 - скорость изменения x
 
% Дифференцируем систему по переменным V teta x y
JA=jacobian(Syst, [V teta x y]);
% Подставляем точку равновесия
A_Syst = subs(JA,[V; teta; x; y; P; alfa; g],[X01; U01; 9.81]);
A=eval(A_Syst)
 
% Дифференцируем систему по переменным P alfa
JB=jacobian(Syst, [P alfa]);
% Подставляем точку равновесия
B_Syst = subs(JB,[V; teta; x; y; P; alfa; g],[X01; U01; 9.81]);
B=eval(B_Syst)
 
% Задаем матрицы С и D
% Выходы берем x и y
C = [0 0 1 0 ;
     0 0 0 1 ]
% Входы на выходы напрямую не действуют
D = [0 0;
     0 0]   
 
% Получили систему в отклонениях dX = A*X + B*U, Y = C*X + D*U
% где X = [ V-97.5;  teta;  x - 1000-97.5*t;  y - 2000]
%     U = [ P-292.782; alfa-0.101 ]