1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
|
import math, numpy as np, scipy.integrate
import matplotlib.pyplot as plt
import pdb
import dynamics3 as dyn
import utils as ut
np.set_printoptions(precision=3, suppress=True, linewidth=200)
def get_trim(aircraft, h, Ma, sm, km):
'''
Calcul du trim pour un point de vol
'''
aircraft.set_mass_and_static_margin(km, sm)
va = dyn.va_of_mach(Ma, h)
Xe, Ue = dyn.trim(aircraft, {'va':va, 'h':h, 'gamma':0})
return Xe, Ue
def NonLinearTraj(aircraft, h, Ma, sm, km, Wh, time):
'''
Affichage d'une trajectoire avec un point de trim comme condition initiale
'''
aircraft.set_mass_and_static_margin(km, sm)
va = dyn.va_of_mach(Ma, h)
Xe, Ue = dyn.trim(aircraft, {'va':va, 'h':h, 'gamma':0})
Xe[dyn.s_a]= Xe[dyn.s_a]+math.atan(Wh/Xe[dyn.s_va])
X = scipy.integrate.odeint(dyn.dyn, Xe, time, args=(Ue, aircraft))
return X
def LinearDyn(X, t, U, A, B):
return np.dot(A,X) + np.dot(B,U)
def LinearTraj(aircraft, h, Ma, sm, km, Wh, time):
'''
Calcul d'une trajectoire avec un point de trim comme condition initiale
'''
aircraft.set_mass_and_static_margin(km, sm)
va = dyn.va_of_mach(Ma, h)
Xe, Ue = dyn.trim(aircraft, {'va':va, 'h':h, 'gamma':0})
A, B = ut.num_jacobian(Xe, Ue, aircraft, dyn.dyn)
dX0 = np.zeros(dyn.s_size)
dU = np.zeros(dyn.i_size)
dX0[dyn.s_a] = math.atan(Wh/Xe[dyn.s_va]) #perturbation
dX = scipy.integrate.odeint(LinearDyn, dX0, time, args=(dU, A, B))
Xlin = np.array([dXi+Xe for dXi in dX])
return Xlin
def get_linearized_model(aircraft, h, Ma, sm, km):
aircraft.set_mass_and_static_margin(km, sm)
va = dyn.va_of_mach(Ma, h)
Xe, Ue = dyn.trim(aircraft, {'va':va, 'h':h, 'gamma':0})
A, B = ut.num_jacobian(Xe, Ue, aircraft, dyn.dyn)
print('poles dim6', np.linalg.eig(A))
Al, Bl = A[dyn.s_va:, dyn.s_va:], B[dyn.s_va:, :dyn.i_wy]
poles, vect_p = np.linalg.eig(Al)
return Al, Bl, poles, vect_p
# division selon puissances croissantes
def div_inc_pow(num, den, order):
rnum = np.zeros(len(den))
for i in range(0,len(num)): rnum[i] = num[-i-1]
rden = den[::-1]
res = np.zeros(order)
for i in range(0, order):
quot, rem = np.polydiv(rnum, rden)
res[i], rnum = quot, np.zeros(len(den))
for i in range(0,len(rem)):
rnum[i] = rem[i]
return res[::-1]
#------------------------
aircraft = dyn.Param_A320()
def run_trajecotry(time, h, Ma, ms, km, Wh=2):
Xe, Ue = get_trim(aircraft, h, Ma, ms, km)
fmt = r'Trim: h={:.1f}, Ma={:.1f}, ms={:.1f}, km={:.1f} -> $\alpha$={:.1f}, $\delta_{{PHR}}$={:.1f}, $\delta_{{th}}$={:.1f}'
print(fmt.format(h, Ma, ms, km, ut.rad_of_deg(Xe[dyn.s_a]), ut.rad_of_deg(Ue[dyn.i_dm]), Ue[dyn.i_dth]))
X = NonLinearTraj(aircraft, h, Ma, ms, km, Wh, time)
Xlin = LinearTraj(aircraft, h, Ma, ms, km, Wh, time)
figure = dyn.plot(time, X)
dyn.plot(time, Xlin, figure=figure)
plt.legend(['non lineaire', 'lineaire'])
h, Ma, ms, km = 3000, 0.5, 0.2, 0.1
run_trajecotry(np.arange(0, 10, 0.1), h, Ma, ms, km, Wh=2)
run_trajecotry(np.arange(0, 240, 0.1), h, Ma, ms, km, Wh=2) |
