(* Лабораторная работа по курсу САУЛА*)
(* Маркелов Н.В. *)

(* Задача: Вывести уравнения движения ЛА *)
(* Для вывода уравнений движения ЛА нам понадобится посчитать матрицы перехода: 
1.Из нормальной СК в связанную
2.Из нормальной СК в скоростную
3.Из нормальной СК в траекторную
4.Из траекторной в скоростную
5.Из скоростной в связанную *)
(* На первом этапе посчитаем матрицу перехода из нормальной системы координат в связанную *)

(* Поворот №1 *)
TableForm[mX={{Cos[ψ[t]], 0, -Sin[ψ[t]]}, {0, 1, 0}, {Sin[ψ[t]], 0, Cos[ψ[t]]}}]

(* Поворот №2 *)
TableForm[mY={{Cos[ϑ[t]], Sin[ϑ[t]], 0}, {-Sin[ϑ[t]], Cos[ϑ[t]], 0}, {0, 0, 1}}]

(* Поворот №3 *)
TableForm[mZ={{1, 0, 0}, {0, Cos[γ[t]], Sin[γ[t]]}, {0, -Sin[γ[t]], Cos[γ[t]]}}]

(* Матрица перехода из нормальной СК в связанную *)
TableForm[mNormToSv=mZ.mY.mX]

(* Переход из нормальной СК в скоростную СК *)
(* Получим путем из матрицы перехода нормальная-связаная путем замены ψ, ϑ, γ на Ψ, Θ, γa *)
TableForm[ mNormToSk=ReplaceAll[mNormToSv, {ψ[t]-> Ψ[t], ϑ[t] -> Θ[t], γ[t] -> bγa[t]}]]

(* Переход из нормальной СК в траекторную СК *)
(* Матрицу можно получить из матрицы перехода нормальная-скоросная приняв γa=0 *)
TableForm[ mNormToTr = Simplify[mNormToSk, bγa[t] == 0]]

(* Переход из траекторной СК в скоростную СК *)
(* Получим из матрицы поворота №3 нормальная-скоросная приняв γ=γa *)
TableForm[ mTrToSk=ReplaceAll[mZ, γ[t] -> bγa[t]]]

(* Переход из скоростной СК в связанную СК *)
(* Получим из матрицы перехода нормальная-траекторная заменив Ψ, Θ на α, β  *)
TableForm[ mSkToSv=ReplaceAll[mNormToTr, {Ψ[t]->  β[t], Θ[t] -> α[t]}]]

(* Кинематические Уравнения Движения ЦМ *)
(* Допущения: конструкция ЛА абсолютно жесткая, атмосфера спокойная, справедлива гипотеза стационарности, Земля плоская, пренеблежем вращением Земли *)
mr=Sqrt[l[t]^2 + h[t]^2 + z[t]^2]
q1=Vx[t]*Cos[ Θ[t]]*Cos[Ψ[t]]
q2=Vy[t]*Sin[ Θ[t]]
q3=Vz[t]*Cos[ Θ[t]]*Sin[Ψ[t]]

TableForm[lr={l[t],h[t],z[t]}]
TableForm[rk={q1,q2,q3}]
TableForm[eq1 = Table [D[lr[[i]], t] == rk[[i]], {i, 1, 3}]]

(* ДИНАМИЧЕСКИЕ УРАВНЕНИЯ ДВИЖЕНИЯ ЦМ *)
(* Сила тяжести в нормальной СК *)
TableForm[fGn = {0, -fmg[t], 0}]

(*  Сила Тяги в связаной СК *)
TableForm[fPsv = {fP[t], 0, 0}]

(*  Аэродинамическая сила в скоростной СК *)
TableForm[fRa = {-fRx[t], fRy[t], fRz[t]}]

(* Силы в связанной СК *)

TableForm[fG1=mNormToSv.fGn]

TableForm[fR1=Transpose[mNormToSv].fRa]

(*  Запишем проекции вектора линейной и угловой скорости в связанной СК *)
TableForm[ dV = {Vx[t], Vy[t], Vz[t]}]

TableForm[ ww = {wx[t], wy[t], wz[t]}]

TableForm[ldV =D[dV,t]+Cross[ww, dV]]

TableForm[eq2 = Table[m*ldV[[i]] == fG1[[i]]+fPsv[[i]]+fRa[[i]],{i,1,3}]]

(*  ДИНАМИЧЕСКИЕ УРАВНЕНИЯ ДВИЖЕНИЯ ВОКРУГ ЦМ *)
(* Составим тензор инерции для ЛА имеющего две плоскости симметрии *)
TableForm[sJ={{sJx, -sJxy, 0}, {-sJxy, sJy, 0}, {0,0,sJz}}]
TableForm[dL=sJ.ww]
Simplify[TableForm[ldL=D[dL, t]+Cross[ww,dL]]]
TableForm[dM={dMx[t], dMy[t], dMz[t]}]
TableForm[ eq3 = Table[Simplify[ldL[[i]]==dM[[i]]],{i,1,3}]]

(*  КИНЕМАТИЧЕСКИЕ УРАВНЕНИЯ ВРАЩАТЕЛЬНОГО ДВИЖЕНИЯ *)
TableForm[ wn = D[{ψ[t], ϑ[t], γ[t]},t]]

(*  Матрица перехода от непрямоугольной к связанной *)
TableForm[mNprToSv = {{Sin[ϑ[t]],0,1},{Cos[ϑ[t]]*Cos[γ[t]],Sin[γ[t]],0},
{-Cos[ϑ[t]]*Cos[γ[t]],Sin[γ[t]],0},{-Cos[ϑ[t]]*Sin[γ[t]],cos[γ[t]],0}}]

TableForm[w={wx[t], wy[t],wz[t]}]

TableForm[Table[w[[i]]==(mNprToSv.wn)[[i]],{i,1,3}]]

(*  Обратная зависимость *)
TableForm[ eq4=Table[wn[[i]]==Simplify[(mNprToSv.w)][[i]],{i,1,3}]]

(*Геометрическая зависимость между углами Ψ, Θ, γa и ψ, ϑ, γ, α, β *)
(* Матрица перехода от нормальной к скоростной СК, через переход от нормальной к связанной СК и от свзязанной к скоростной СК *)
TableForm[ mNormToSk1=Transpose[mNormToSv].mNormToSv]

TableForm[ en1={mNormToSv[[1,2]],mNormToSv[[1,3]],mNormToSv[[3,2]]}]

TableForm[ en2={mNormToSk1[[1,2]],mNormToSk1[[1,3]],mNormToSk1[[3,2]]}]

TableForm[Table[en1[[i]]==en2[[i]],{i,1,3}]]

(* УРАВНЕНИЯ ТЯГИ ДВИГАТЕЛЯ  *)
dm=m[t]

q5=mrtop[t]

D[dm,t]==-Abs[D[q5,t]]

m'[t]==-Abs[mrtop'[t]]
(* Уравнение для числа маха *)

m'[t]==-Abs[mrtop'[t]]

(*  ОБЩИЕ УРАВНЕНИЯ ДВИЖЕНИЯ ЛА *)
(* Общие нелинейные уравнения движения *)

TableForm[eq0=Join[eq1,eq2,eq3,eq4]]

TableForm[var={l'[t],h'[t],z'[t],Vx'[t],Vy'[t],Vz'[t],wx'[t],wy'[t],wz'[t],ψ'[t],ϑ'[t],γ'[t]}]

TableForm[eq0]

TableForm[var]

(* Норамльная форма Коши *)

TableForm[eq00=Flatten[Simplify[Solve[eq0, var]]]]

TableForm[eq00]

eq00[[1]]

TableForm[rt=Table[eq00[[i,1]],{i,1,Length[eq00]}]]

TableForm[rt=Table[eq00[[i,2]],{i,1,Length[eq00]}]]

eq00[[1,2]]

TableForm[eq000=Table[eq00[[i,1]]==eq00[[i,2]],{i,1,Length[eq00]}]]

TableForm[ReplaceAll[eq000, {ψ[t]->0,ϑ[t]->0, γ[t]->0, Vx[t]->0,Vy[t]->0,
Vz[t]->0,α[t]->0, β[t]->0,Ψ[t]->0, Θ[t]->0, wx[t]->0,wy[t]->0,wz[t]->0}]]

(*  ОПРЕДЕЛИМ КОЭФФИЦИЕНТЫ ЛИНЕЙНОЙ МОДЕЛИ *)

eq11:= ReplaceAll[eq000,{
fP[t]->fP[mah[t],δPdv[t]],
fX[t]->fX[α[t],mah[t]],
fY[t]->fY[α[t]],
fZ[t]->fZ[β[t],mah[t]],
dMx[t]->dMx[β[t],δE[t]],
dMy[t]->dMy[δH[t]],
dMz[t]->dMz[δB[t],α[t]]}]

TableForm[eq11]

TableForm[rt1=Table[eq11[[i,2]],{i,1,Length[eq11]}]]

TableForm[lt1={l[t],h[t],z[t],Vx[t],Vy[t],Vz[t],wx[t],wy[t],wz[t],ψ[t],ϑ[t],γ[t]}]

TableForm[krt=Table[D[rt1[[i]],lt1[[j]]],{i,1,Length[rt1]},{j,1,Length[lt1]}]]

Print[krt[[7,7]]]

TableForm[ult={δB[t],δH[t],δE[t],δPdv[t]}]

TableForm[k=Table[D[rt1[[i]],ult[[j]]],{i,1,Length[rt1]},{j,1,Length[ult]}]]

Table[Print["u(",i,",",j,")=",k[[j,i]]],{i,1,2},{j,1,12}]