Модель ракеты с системой самонаведения
Author
Co-authors
Thrust = 10000.0
g = 9.81
wn_hom = 7.0
tors = 1/20.0
max_gimbal = deg2rad( 35 )
min_gimbal = deg2rad( -35 )
wgyro = 100 * 2π
Ks = wgyro / 5
K_r = -0.02
Beamwidth = deg2rad( 10 )
max_acc = 40g
# Параметры объекта управления
x_init = 0.0
h_init = 3048.0
v_init = 3 * 328.0
theta_init = 0.0
q_init = 0.0
alpha_init = 0.0
mass = 204.02
Iyy = 247.44
# Параметры цели
theta_tgt = deg2rad( 180 );
pos_tgt = [4500.0+x_init, -h_init-500]
v_tgt = 328.0
wn_fin = 150.0
z_fin = 0.7
fin_act_0 = 0.0
fin_max = deg2rad( 30 )
fin_min = deg2rad( -30 )
fin_maxrate = deg2rad( 500 )
M_sch = [ 2.0000, 2.2500, 2.5000, 2.7500, 3.0000, 3.2500, 3.5000, 3.7500, 4.0000 ]
alpha_sch = [0, 0.0698, 0.1396, 0.2094, 0.2793, 0.3491]
Ka = [
0.9412 0.9389 0.9352 0.9321 0.9273 0.9217;
0.9364 0.9336 0.9300 0.9266 0.9203 0.9142;
0.9316 0.9290 0.9250 0.9204 0.9137 0.9071;
0.9269 0.9239 0.9201 0.9146 0.9075 0.9005;
0.9220 0.9191 0.9148 0.9092 0.9016 0.8943;
0.9173 0.9140 0.9099 0.9042 0.8962 0.8885;
0.9131 0.9092 0.9047 0.8995 0.8911 0.8831;
0.9084 0.9047 0.8998 0.8947 0.8864 0.8781;
0.9040 0.9005 0.8952 0.8903 0.8820 0.8735;
]
K = [
0.0259 0.0249 0.0237 0.0229 0.0218 0.0207;
0.0212 0.0204 0.0195 0.0188 0.0176 0.0168;
0.0178 0.0172 0.0164 0.0156 0.0147 0.0139;
0.0151 0.0146 0.0140 0.0132 0.0124 0.0118;
0.0130 0.0126 0.0120 0.0114 0.0107 0.0102;
0.0113 0.0109 0.0105 0.0100 0.0094 0.0089;
0.0100 0.0096 0.0092 0.0088 0.0083 0.0079;
0.0089 0.0085 0.0082 0.0079 0.0074 0.0071;
0.0079 0.0077 0.0073 0.0071 0.0067 0.0064;
]
Ki = [
22.5220 22.7092 22.9250 23.0923 23.2089 23.2709;
26.1087 26.3788 26.6897 26.9316 27.0991 27.1852;
30.1257 30.5069 30.9480 31.2933 31.5308 31.6508;
34.5616 35.0914 35.7077 36.1912 36.5201 36.6881;
39.3999 40.1205 40.9646 41.6291 42.0832 42.3099;
44.6135 45.5767 46.7100 47.6092 48.2235 48.5294;
50.5274 51.4311 52.9278 54.1228 54.9412 55.3466;
56.4127 57.6462 59.5894 61.1489 62.2223 62.7535;
62.5431 64.1724 66.6498 68.6538 70.0408 70.7266;
]
Kg = [
0.8503 0.8430 0.8350 0.8290 0.8252 0.8234;
0.6688 0.6616 0.6535 0.6475 0.6437 0.6419;
0.5397 0.5324 0.5244 0.5184 0.5145 0.5127;
0.4448 0.4375 0.4294 0.4234 0.4196 0.4178;
0.3731 0.3658 0.3578 0.3518 0.3479 0.3461;
0.3179 0.3106 0.3025 0.2965 0.2926 0.2908;
0.2739 0.2672 0.2591 0.2531 0.2492 0.2474;
0.2394 0.2327 0.2246 0.2186 0.2147 0.2128;
0.2116 0.2049 0.1968 0.1907 0.1868 0.1850;
]
Cm_q = -1.719
S_ref = 0.040877
d_ref = 0.2286
Cz_el = -1.9481
Cm_el = -11.803
alpha_vec = [ -0.3491, -0.3316, -0.3142, -0.2967, -0.2793, -0.2618, -0.2443, -0.2269, -0.2094, -0.1920, -0.1745, -0.1571, -0.1396, -0.1222, -0.1047, -0.0873, -0.0698, -0.0524, -0.0349,-0.0175, 0, 0.0175, 0.0349, 0.0524, 0.0698, 0.0873, 0.1047, 0.1222, 0.1396, 0.1571, 0.1745, 0.1920, 0.2094, 0.2269, 0.2443, 0.2618, 0.2793, 0.2967, 0.3142, 0.3316, 0.3491]
Mach_vec = [2.0000, 2.5000, 3.0000, 3.5000, 4.0000]
Cx_alpha = -0.3 .* ones( length(alpha_vec), length(Mach_vec) )
Cz_alpha = [
7.4787 6.9133 6.3480 5.7827 5.2173;
7.0015 6.4644 5.9274 5.3903 4.8532;
6.5315 6.0227 5.5139 5.0051 4.4963;
6.0693 5.5887 5.1082 4.6277 4.1471;
5.6154 5.1632 4.7109 4.2586 3.8064;
5.1706 4.7466 4.3226 3.8986 3.4746;
4.7354 4.3397 3.9440 3.5482 3.1525;
4.3105 3.9430 3.5756 3.2081 2.8406;
3.8964 3.5572 3.2180 2.8788 2.5396;
3.4938 3.1829 2.8720 2.5610 2.2501;
3.1033 2.8207 2.5380 2.2553 1.9727;
2.7256 2.4712 2.2168 1.9624 1.7080;
2.3611 2.1350 1.9089 1.6827 1.4566;
2.0107 1.8128 1.6149 1.4171 1.2192;
1.6748 1.5052 1.3356 1.1660 0.9964;
1.3540 1.2127 1.0714 0.9300 0.7887;
1.0491 0.9361 0.8230 0.7099 0.5969;
0.7607 0.6759 0.5911 0.5063 0.4215;
0.4892 0.4327 0.3762 0.3196 0.2631;
0.2355 0.2072 0.1789 0.1507 0.1224;
0 0 0 0 0;
-0.2355 -0.2072 -0.1789 -0.1507 -0.1224;
-0.4892 -0.4327 -0.3762 -0.3196 -0.2631;
-0.7607 -0.6759 -0.5911 -0.5063 -0.4215;
-1.0491 -0.9361 -0.8230 -0.7099 -0.5969;
-1.3540 -1.2127 -1.0714 -0.9300 -0.7887;
-1.6748 -1.5052 -1.3356 -1.1660 -0.9964;
-2.0107 -1.8128 -1.6149 -1.4171 -1.2192;
-2.3611 -2.1350 -1.9089 -1.6827 -1.4566;
-2.7256 -2.4712 -2.2168 -1.9624 -1.7080;
-3.1033 -2.8207 -2.5380 -2.2553 -1.9727;
-3.4938 -3.1829 -2.8720 -2.5610 -2.2501;
-3.8964 -3.5572 -3.2180 -2.8788 -2.5396;
-4.3105 -3.9430 -3.5756 -3.2081 -2.8406;
-4.7354 -4.3397 -3.9440 -3.5482 -3.1525;
-5.1706 -4.7466 -4.3226 -3.8986 -3.4746;
-5.6154 -5.1632 -4.7109 -4.2586 -3.8064;
-6.0693 -5.5887 -5.1082 -4.6277 -4.1471;
-6.5315 -6.0227 -5.5139 -5.0051 -4.4963;
-7.0015 -6.4644 -5.9274 -5.3903 -4.8532;
-7.4787 -6.9133 -6.3480 -5.7827 -5.2173;
]
Cm_alpha = [
7.7800 6.4200 5.0600 3.7000 2.3400;
7.1798 5.8878 4.5958 3.3038 2.0118;
6.5941 5.3701 4.1461 2.9221 1.6981;
6.0242 4.8682 3.7122 2.5562 1.4002;
5.4714 4.3834 3.2954 2.2074 1.1194;
4.9369 3.9169 2.8969 1.8769 0.8569;
4.4220 3.4700 2.5180 1.5660 0.6140;
3.9281 3.0441 2.1601 1.2761 0.3921;
3.4565 2.6405 1.8245 1.0085 0.1925;
3.0083 2.2603 1.5123 0.7643 0.0163;
2.5850 1.9050 1.2250 0.5450 -0.1350;
2.1878 1.5758 0.9638 0.3518 -0.2602;
1.8179 1.2739 0.7299 0.1859 -0.3581;
1.4768 1.0008 0.5248 0.0488 -0.4272;
1.1656 0.7576 0.3496 -0.0584 -0.4664;
0.8856 0.5456 0.2056 -0.1344 -0.4744;
0.6382 0.3662 0.0942 -0.1778 -0.4498;
0.4247 0.2207 0.0167 -0.1873 -0.3913;
0.2463 0.1103 -0.0257 -0.1617 -0.2977;
0.1043 0.0363 -0.0317 -0.0997 -0.1677;
0 0 0 0 0;
-0.1043 -0.0363 0.0317 0.0997 0.1677;
-0.2463 -0.1103 0.0257 0.1617 0.2977;
-0.4247 -0.2207 -0.0167 0.1873 0.3913;
-0.6382 -0.3662 -0.0942 0.1778 0.4498;
-0.8856 -0.5456 -0.2056 0.1344 0.4744;
-1.1656 -0.7576 -0.3496 0.0584 0.4664;
-1.4768 -1.0008 -0.5248 -0.0488 0.4272;
-1.8179 -1.2739 -0.7299 -0.1859 0.3581;
-2.1878 -1.5758 -0.9638 -0.3518 0.2602;
-2.5850 -1.9050 -1.2250 -0.5450 0.1350;
-3.0083 -2.2603 -1.5123 -0.7643 -0.0163;
-3.4565 -2.6405 -1.8245 -1.0085 -0.1925;
-3.9281 -3.0441 -2.1601 -1.2761 -0.3921;
-4.4220 -3.4700 -2.5180 -1.5660 -0.6140;
-4.9369 -3.9169 -2.8969 -1.8769 -0.8569;
-5.4714 -4.3834 -3.2954 -2.2074 -1.1194;
-6.0242 -4.8682 -3.7122 -2.5562 -1.4002;
-6.5941 -5.3701 -4.1461 -2.9221 -1.6981;
-7.1798 -5.8878 -4.5958 -3.3038 -2.0118;
-7.7800 -6.4200 -5.0600 -3.7000 -2.3400;
]
l_acc = 0.5;