1. Линеаризация уравнения нормальных сил

Динамические уравнения вращательного движения

MatrixForm[m1={{Cos[ψ],0,-Sin[ψ]},{0,1,0},{Sin[ψ],0,Cos[ψ]}}]

MatrixForm[m2={{Cos[ϑ],Sin[ϑ],0},{-Sin[ϑ],Cos[ϑ],0},{0,0,1}}]

MatrixForm[m3={{1,0,0},{0,Cos[γ],Sin[γ]},{0,-Sin[γ],Cos[γ]}}]

MatrixForm[Mlg=m3.m2.m1] (*Матрица перехода из нормальной в связанную*)

MatrixForm[Mag=ReplaceAll[Mlg,{ψ->Ψ,ϑ->Θ,γ->Γ}]] (*Матрица перехода из нормальной в скоростную*)

MatrixForm[Mkg=ReplaceAll[Mag,{Γ->0}]] (*Матрица перехода из нормальной в траекторную*)

MatrixForm[Mak=ReplaceAll[m3,{γ->Γ}]] (*Матрица перехода из нормальной в скоростную*)

MatrixForm[Mla=ReplaceAll[Mkg,{Ψ->α,Θ->β}]] (*Матрица перехода из скоростной в связанную*)

Обычно уравнения сил проецируют на оси траекторной 
системы координат,так как в это случае Vx=V,
Vy=Vz=0 и уравнения получают в достаточно
 простом виде.

Выразим координаты всех сил в базисе траекторной системы

1. Сила тяжести.Поскольку эта сила направлена по местной вертикали,её обычно задают в базисе нормальной системы координат:

VG={0,-G,0};

2. Сила тяги двигателя приложена в центре масс двигателя и направлена по оси OX
связанной системы.Она задается в базисе связанной системы координат

VP={P,0,0}

3. Равнодействующая аэродинамических сил обычно записывается в базисе
скоростной системы:

VR={-X,Y,Z}

где X – сила лобового сопротивления;
Y – подъемная сила;
Z – боковая сила

Запишем эти силы в проекциях на оси траекторной системы:

MatrixForm[VGt=Mkg.VG]

MatrixForm[VPt=Transpose[Mak].Transpose[Mla].VP]

Аэродинамические силы выразятся так:

MatrixForm[VRt=Transpose[Mak].VR]

Clear[V];
Vt={V[t],0,0} (*Вектор скорости*)

wb={D[Ψ[t],t]*Sin[Θ[t]],D[Ψ[t],t]*Cos[Θ[t]],D[Θ[t],t]} (*Скорость вращения базиса*)

dvtdt=D[Vt,t]+Cross[wb,Vt] (*Производная вектора скорости*)

(*----Уравнения сил в проекциях на оси траекторной системы-----*)
MatrixForm[m*dvtdt]==MatrixForm[VGt+VPt+VRt]

Исходное основное уравнение

IOU=m*dvtdt[[2]]==(VGt+VPt+VRt)[[2]]

Исходное возмущенное уравнение:
Параметры возмущенного движения отличаются от параметров невозмущенного
движения на некоторую малую величину
Придадим параметрам их приращения

Ψ1[t]=Ψ0[t]+dΨ[t];
Θ1[t]=Θ0[t]+dΘ[t];
α1[t]=α0[t]+dα[t];
β1[t]=β0[t]+dβ[t];
Γ1[t]=Γ0[t]+dΓ[t];
P1=P0+dP;

wb1={D[Ψ1[t],t]*Sin[Θ1[t]],D[Ψ1[t],t]*Cos[Θ1[t]],D[Θ1[t],t]};
Vt1={V0[t]+dV[t],0,0};
dvtdt1=D[Vt1,t]+Cross[wb1,Vt1];

pr=(VGt+VPt+VRt)[[2]];

prch=ReplaceAll[pr,{Θ->Θ1[t],Ψ->Ψ1[t],α->α1[t],β->β1[t],Γ->Γ1[t]}] (*Правая часть уравнения*)

IVU=m*dvtdt1[[2]]==prch

Получаем уравнение в вариациях:

UrVD=FullSimplify[IVU[[1]]-IOU[[1]]]==FullSimplify[IVU[[2]]-IOU[[2]]]

Разложим все функции в последнем уравнении в ряд Тейлора по степеням
приращений H ΔV,...,Δδ и оставим члены порядка малости не выше первой

sina=Normal[Series[Sin[dα[t]+α0[t]],{dα[t],0,1}]];
sinb=Normal[Series[Sin[dβ[t]+β0[t]],{dβ[t],0,1}]];
sing=Normal[Series[Sin[dΓ[t]+Γ0[t]],{dΓ[t],0,1}]];
cosg=Normal[Series[Cos[dΓ[t]+Γ0[t]],{dΓ[t],0,1}]];
cosb=Normal[Series[Cos[dβ[t]+β0[t]],{dβ[t],0,1}]];
cost=Normal[Series[Cos[dΘ[t]+Θ0[t]],{dΘ[t],0,1}]];

FullSimplify[UrVD=ReplaceAll[UrVD,{Sin[dβ[t]+β0[t]]->sinb, Sin[dΓ[t]+Γ0[t]]->sing, Sin[dα[t]+α0[t]]->sina, Cos[dΓ[t]+Γ0[t]]->cosg, Cos[dβ[t]+β0[t]]->cosb, Cos[dΘ[t]+Θ0[t]]->cost}]]

Пренебрежем вторым порядком малости, т.е. произведения вариаций и их степени выше 1-й приравняем к нулю, также a0 и b0 невелики -> sin(a0)=a0, sin(B0)=B0, cos(a0)=cos(b0)=1

FullSimplify[(dV[t]+V0[t]) ((dΘ)'[t]+(Θ0)'[t])]

FullSimplify[UrVD=ReplaceAll[UrVD,{Sin[α0[t]]->α0[t], Sin[β0[t]]->β0[t], Cos[β0[t]]->1, Cos[α0[t]]->1, Cos[Γ0[t]] dΓ[t] ->0, (dV[t]+V0[t]) ((dΘ)'[t]+(Θ0)'[t])->0}]]

FullSimplify[Normal[Series[UrVD[[1]],{dV[t],0,1}]]]

FullSimplify[Normal[Series[UrVD[[2]],{dV[t],0,1}]]]

w={wx1[t],wy1[t],wz1[t]};
MatrixForm[K=TI.w];
M={Mx1,My1,Mz1};
Mz1=Mrz1-P1*yp*Cos[fip];
wx1[t]=wx0[t]+dwx[t];
wy1[t]=wy0[t]+dwy[t];
wz1[t]=wz0[t]+dwz[t];

IVU=D[K,t][[3]]+Cross[w,K][[3]]==M[[3]]
-Ixy ((dwx[t])^2)-Ixx dwx[t] dwy[t]+Iyy dwx[t] dwy[t]+Ixy ((dwy[t])^2)-2 Ixy dwx[t] wx0[t]-Ixx dwy[t] wx0[t]+Iyy dwy[t] wx0[t]-Ixy ((wx0[t])^2)-Ixx dwx[t] wy0[t]+Iyy dwx[t] wy0[t]+2 Ixy dwy[t] wy0[t]-Ixx wx0[t] wy0[t]+Iyy wx0[t] wy0[t]+Ixy ((wy0[t])^2)+Izz (dwz'[t]+wz0'[t])==Mrz0+dalpha Mrz'[alpha]+drv Mrz'[drv]+dzak Mrz'[dzak]+((dv Mrz'[Mah])/a)+wz Mrz'[wz]+alpha'[t] Mrz'[alpha'[t]]-yp Cos[fip] (P0+dsg P'[dsg]+dv P'[dv])

Получаем возмущенное уравнение момента тангажа, т.е. уравнение в вариациях:

UrVD=IVU[[1]]-IOU[[1]]==Simplify[IVU[[2]]-IOU[[2]]]

w={wx[t],wy[t],wz[t]} (*Вектор угловой скорости ЛА в связанной системе координат*)

MatrixForm[K=TI.w] (*Главный момент количеств движения в связанной системе координат*)

M={Mx,My,Mz} (*Результирующий момент аэродинамических сил и тяги в связанной системе координат(Момент силы тяжести, центробежных и кориолисовых сил инерции вокруг центра масс ЛА равен нулю)*)

Mz=Mrz-P*yp*Cos[fip]

lchudvd=D[K,t]+Cross[w,K] (*Полная производная по времени главного вектора момента количеств движения*)

Полная производная главного вектора момента количеств движения равна главному моменту внешних сил, действующих на тело.

UrT=lchudvd[[3]]==M[[3]] (*Уравнение момента тангажа*)

-Ixy ((wx[t])^2)-Ixx wx[t] wy[t]+Iyy wx[t] wy[t]+Ixy ((wy[t])^2)+Izz wz'[t]==Mrz-P yp Cos[fip]

Clear[vx,vy,vz];
V={vx[t],vy[t],vz[t]}; (*вектор скорости ЛА*)
Q=m*D[V,t]

R={}

Уравнения движения центра масс ЛА получим основании закона изменения количества движения центра масс: dQ/dt={Fi

Исходное основное уравнение момента тангажа :

w={wx0[t],wy0[t],wz0[t]};
MatrixForm[K=TI.w];
M={Mx0,My0,Mz0};
Mz0=Mrz0-P0*yp*Cos[fip];
IOU=D[K,t][[3]]+Cross[w,K][[3]]==M[[3]]

Исходное возмущенное уравнение момента тангажа :

w={wx1[t],wy1[t],wz1[t]};
MatrixForm[K=TI.w];
M={Mx1,My1,Mz1};
Mz1=Mrz1-P1*yp*Cos[fip];
wx1[t]=wx0[t]+dwx[t];
wy1[t]=wy0[t]+dwy[t];
wz1[t]=wz0[t]+dwz[t];
Mrz1=Mrz0+dMrz;
P1=P0+dP;
IVU=D[K,t][[3]]+Cross[w,K][[3]]==M[[3]]

Получаем возмущенное уравнение момента тангажа, т.е. уравнение в вариациях:

UrVD=IVU[[1]]-IOU[[1]]==Simplify[IVU[[2]]-IOU[[2]]]

Исходное основное движение ЛА - прямолинейный горизонтальный полет: wx0[t]=0, wy0[t]=0 ,wz0[t]=0, wz0'[t]=0.
Пренебрежем вторым порядком малости, т.е. произведения вариаций и их степени выше 1-й приравняем к нулю.

UrVD=ReplaceAll[UrVD,{wx0[t]->0,wy0[t]->0,wz0[t]->0,wz0'[t]->0,((dwx[t])^2)->0,((dwy[t])^2)->0,dwx[t] dwy[t]->0}]

Т.к. силы и моменты зависят от параметров полета и соответственно, на их величины влияют частные производные от этих параметров, то можно записать:

dMrz=D[Mrz[wz],wz]*wz+D[Mrz[alpha],alpha]*dalpha+D[Mrz[Mah],Mah]*dv/a+D[Mrz[D[alpha[t],t]],D[alpha[t],t]]*D[alpha[t],t]+D[Mrz[drv],drv]*drv+D[Mrz[dzak],dzak]*dzak

dP=D[P[dsg],dsg]*dsg+D[P[dv],dv]*dv

Уточненное уравнение имеет вид:

UrVD

Вычтем из левой части уравнения правую и разделим получившееся уравнение на коэффициент при производной угловой скорости тангажа:

UrVD=(UrVD[[1]]-UrVD[[2]])/Izz==0

Clear[M,P]
UrVD=ReplaceAll[UrVD,{Mrz'[alpha]->(Subscript[M, z]^α),Mrz'[drv]->(Subscript[M, z]^(Subscript[δ, В])),Mrz'[dzak]->(Subscript[M, z]^(Subscript[δ, З])),Mrz'[Mah]->(Subscript[M, z]^M),Mrz'[wz]->(Subscript[M, z]^(Subscript[ω, z])),P'[dsg]->(P^(Subscript[δ, СГ])),P'[dv]->(P^v),Mrz'[alpha'[t]]->(Subscript[M, z]^(OverDot[α]))}]

Линеаризованное уравнение момента тангажа:

U1=({(Subscript[(OverDot[ω]), z]),(Subscript[ω, z]),(OverDot[α]),α,Δv,(Subscript[δ, В]),(Subscript[δ, СГ]),(Subscript[δ, З])}.{{1},{c1},{c5},{c2},{e3},{с3},{r3},{c12}})[[1]]==0

Коэффициенты линейного уравнения:

c1=D[UrVD[[1]],wz]

c2=D[UrVD[[1]],dalpha]

с3=D[UrVD[[1]],drv]

c5=D[UrVD[[1]],alpha'[t]]

c12=D[UrVD[[1]],dzak]

e3=D[UrVD[[1]],dv]

r3=D[UrVD[[1]],dsg]

U2=({(Subscript[(OverDot[ω]), z]),(Subscript[ω, z]),(OverDot[α]),α,Δv,(Subscript[δ, В]),(Subscript[δ, СГ]),(Subscript[δ, З])}.{{1},{c1},{c5},{c2},{e3},{с3},{r3},{c12}})[[1]]==0

Запишем коэффициенты в принятой форме :

c1=ReplaceAll[c1,(Subscript[M, z]^(Subscript[ω, z]))->(Subscript[m, z]^(Subscript[ω, z]))((ρV)/2)(Subscript[Sb, A]^2)]

c2=ReplaceAll[c2,(Subscript[M, z]^α)->(Subscript[m, z]^α)(((ρV)^2)/2)(Subscript[Sb, A])]

с3=ReplaceAll[с3,(Subscript[M, z]^(Subscript[δ, В]))->(Subscript[m, z]^(Subscript[δ, В]))(((ρV)^2)/2)(Subscript[Sb, A])]

c5=ReplaceAll[c5,(Subscript[M, z]^(OverDot[α]))->(Subscript[m, z]^(OverDot[α]))((ρV)/2)(Subscript[Sb, A]^2)]

c12=ReplaceAll[c12,(Subscript[M, z]^(Subscript[δ, З]))->(Subscript[m, z]^(Subscript[δ, З]))(((ρV)^2)/2)(Subscript[Sb, A])]

e3=ReplaceAll[e3,(Subscript[M, z]^M)->(Subscript[m, z]^M)(((ρV)^2)/2)(Subscript[Sb, A])]

Окончательное выражение имеет вид :

U3=({(Subscript[(OverDot[ω]), z]),(Subscript[ω, z]),(OverDot[α]),α,Δv,(Subscript[δ, В]),(Subscript[δ, СГ]),(Subscript[δ, З])}.{{1},{c1},{c5},{c2},{e3},{с3},{r3},{c12}})[[1]]==0