Nghiên cứu, tính toán ổn định và điều khiển tên lửa sử dụng công cụ tính toán số

Tổng quan về tên lửa; phương trình chuyển động của vật rắn; giới thiệu về công cụ tính toán; ứng dụng missile datcom tính toán đặc tính khí động, điều khiển tên lửa; mô phỏng quỹ đạo và khảo sát ổn định. Tổng quan về tên lửa; phương trình chuyển động của vật rắn; giới thiệu về công cụ tính toán; ứng dụng missile datcom tính toán đặc tính khí động, điều khiển tên lửa; mô phỏng quỹ đạo và khảo sát ổn định.

\ LÊ TRẦN THANH HẢI BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LÊ TRẦN THANH HẢI KỸ THUẬT CƠ KHÍ ĐỘNG LỰC NGHIÊN CỨU, TÍNH TỐN ỔN ĐỊNH VÀ ĐIỀU KHIỂN TÊN LỬA SỬ DỤNG CƠNG CỤ TÍNH TỐN SỐ LUẬN VĂN THẠC SĨ KHOA HỌC KỸ THUẬT CƠ KHÍ ĐỘNG LỰC CLC2018B HÀ NỘI – 2019 \ BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LÊ TRẦN THANH HẢI NGHIÊN CỨU, TÍNH TỐN ỔN ĐỊNH VÀ ĐIỀU KHIỂN TÊN LỬA SỬ DỤNG CƠNG CỤ TÍNH TỐN SỐ Chun ngành: Kỹ thuật Cơ khí Động lực LUẬN VĂN THẠC SĨ KHOA HỌC KỸ THUẬT CƠ KHÍ ĐỘNG LỰC NGƯỜI HƯỚNG DẪN KHOA HỌC: TS LƯU HỒNG QUÂN HÀ NỘI – 2019 \ CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc BẢN XÁC NHẬN CHỈNH SỬA LUẬN VĂN THẠC SĨ Họ tên tác giả luận văn: Lê Trần Thanh Hải Đề tài luận văn: Nghiên cứu, tính tốn ổn định điều khiển tên lửa sử dụng cơng cụ tính tốn số Chuyên ngành: Kỹ thuật Cơ khí động lực Mã số HV: CBC18011 Tác giả, Người hướng dẫn khoa học Hội đồng chấm luận văn xác nhận tác giả sửa chữa, bổ sung luận văn theo biên họp Hội đồng ngày 12 / 09 / 2019 với nội dung sau: - Sửa lỗi chế bản, tả - Sửa lại số hình vẽ có phơng chữ nhỏ - Thêm phần giải thích vấn đề thay đổi trọng lượng trình bay chương 2, trang 23 - Phần giải thích cơng thức 2.25 sửa lỗi sai đánh máy - Tài liệu tham khảo trích dẫn dạng ngoặc vng, số mục quan trọng ghi số trang tài liệu tham khảo Ngày tháng năm 2019 Giảng viên hướng dẫn Tác giả luận văn CHỦ TỊCH HỘI ĐỒNG \ LỜI CAM ĐOAN Tôi – Lê Trần Thanh Hải, học viên lớp Cao học Kỹ thuật Cơ khí Động lực khóa CLC2018B Trường Đại học Bách khoa Hà Nội – cam kết luận văn cơng trình nghiên cứu thân hướng dẫn TS Lưu Hồng Quân – Viện Cơ khí Động lực – Đại học Bách khoa Hà Nội Các số liệu, kết nêu luận văn trung thực chưa công bố công trình khác Tác giả luận văn xin chịu trách nhiệm nghiên cứu Hà Nội, ngày tháng năm 2019 Tác giả Lê Trần Thanh Hải \ Xác nhận giáo viên hướng dẫn mức độ hoàn thành luận văn thạc sĩ khoa học cho phép bảo vệ: ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… ………………………………………………………………………………………… Hà Nội, ngày tháng năm 2019 Giảng viên hướng dẫn TS Lưu Hồng Quân \ MỤC LỤC LỜI MỞ ĐẦU CHƯƠNG GIỚI THIỆU CHUNG VỀ TÊN LỬA 1.1 TỔNG QUAN VỀ TÊN LỬA 1.1.1 Khái niệm 1.1.2 Lịch sử hình thành phát triển 1.1.3 Phân loại 1.2 GIỚI THIỆU TÊN LỬA FGM-148 JAVELIN 1.2.1 Lịch sử 1.2.2 Chức 10 1.2.3 Nguyên lý hoạt động 10 1.2.4 Cấu tạo thông số 11 1.3 ĐẶT VẤN ĐỀ 12 CHƯƠNG PHƯƠNG TRÌNH CHUYỂN ĐỘNG CỦA VẬT RẮN 14 2.1 CÁC LỰC KHÍ ĐỘNG VÀ MƠMEN TÁC ĐỘNG LÊN VẬT BAY 14 2.1.1 Các hệ số lực khí động mơmen 14 2.1.2 Các hệ số đạo hàm 15 2.1.3 Lực mômen 15 2.2 PHƯƠNG TRÌNH CHUYỂN ĐỘNG 17 2.2.1.Các hệ trục tọa độ 17 2.2.2 Các phương trình động học 19 2.2.3 Các phương trình động lực học 20 2.3 MÔ PHỎNG CHUYỂN ĐỘNG VẬT BAY BẰNG MƠ HÌNH HÌNH SÁU BẬC TỰ DO 22 CHƯƠNG GIỚI THIỆU VỀ CÔNG CỤ TÍNH TỐN 24 3.1 MISSILE DATCOM 24 3.1.1.Giới thiệu chung 24 3.1.2 Tính 25 3.1.3 Vận hành chương trình 28 3.1.4 Nhập file đầu vào 28 3.1.5 Đọc file đầu 43 3.1.6 Ví dụ minh họa 47 3.2 MATLAB SIMULINK 50 3.2.1 Giới thiệu chung 50 3.2.2 Khởi động Simulink 51 3.2.3 Tạo sơ đồ mô 51 3.2.4 Thiết lập giải mô chạy chương trình 54 \ CHƯƠNG ỨNG DỤNG MISSILE DATCOM TÍNH TỐN ĐẶC TÍNH KHÍ ĐỘNG, ĐIỀU KHIỂN TÊN LỬA 59 4.1 MƠ HÌNH TÍNH TOÁN 59 4.2 PHÂN TÍCH KẾT QUẢ 60 CHƯƠNG MÔ PHỎNG QUỸ ĐẠO VÀ KHẢO SÁT ỔN ĐỊNH 68 5.1 XÂY DỰNG MƠ HÌNH TÍNH TỐN TÊN LỬA BẰNG CƠNG CỤ MATLAB SIMULINK 68 5.2 TÍNH TỐN Q TRÌNH BAY KHÔNG LỆCH CÁNH LÁI 70 5.2.1 Thiết lập giải 70 5.2.2 Kết tính tốn 71 5.2.3 Khảo sát ổn định điều khiển 73 5.2.4 Khảo sát trường hợp trọng tâm nằm sau tâm khí động 76 5.2.5 Ảnh hưởng gió tới quỹ đạo bay 78 5.3 TÍNH TỐN ĐIỀU KHIỂN TÊN LỬA BẰNG MƠ HÌNH PID 79 5.3.1 Điều khiển cơng đột 80 5.3.2 Trường hợp xét đến ảnh hưởng gió ngang gió dọc 83 KẾT LUẬN 87 TÀI LIỆU THAM KHẢO 88 PHỤ LỤC 89 \ DANH MỤC BẢNG BIỂU Bảng Véc tơ khí động học đầu vào 13 Bảng Input file Missile Datcom 27 Bảng Các output file Missile Datcom 27 Bảng Các namelist Datcom 31 Bảng Các biến điều kiện bay 32 Bảng Các biến tham chiếu 32 Bảng Các biến hình học thân 33 Bảng Các biến hình học 34 Bảng Các biến tổ hợp cánh 35 Bảng 10 Nhập biên dạng cánh NACA 36 Bảng 11 Đặt góc mở cánh lái 37 Bảng 12 Cài đặt điều kiện bay ổn định 38 Bảng 13 Cài đặt cánh hút gió 39 Bảng 14 Cài đặt liệu thực nghiệm thay 39 Bảng 15 Cài đặt trình điều khiển 40 Bảng 16 Cài đặt tùy chỉnh đầu 42 Bảng 17 Kí hiệu hệ số file đầu 44 Bảng 18 Kí hiệu hệ số đạo hàm file đầu 47 Bảng 19 Thơng số hình học cánh tên lửa SA-2 48 Bảng 20 Điều kiện bay tên lửa SA-2 48 Bảng 21 So sánh bước thời gian 57 Bảng 22 So sánh kết mô ném xiên với lý thuyết 58 Bảng 23 Các thông số tên lửa Javelin 59 Bảng 24 Các hệ số lực khí động 65 Bảng 25 Các hệ số mômen 65 Bảng 26 Các hệ số đạo hàm theo vận tốc pitch 66 Bảng 27 Các hệ số đạo hàm theo vận tốc yaw 66 Bảng 28 Các hệ số đạo hàm theo vận tốc roll 67 Bảng 29 Thông số khối tên lửa Javelin 68 Bảng 30 Véc tơ trạng thái khởi tạo 69 Bảng 31 So sánh bước thời gian mô cho mơ hình tên lửa 70 Bảng 32.So sánh liệu tính tốn với liệu báo 73 Bảng 33 Giá trị khảo sát ổn định 74 Bảng 34 So sánh tầm xa trước sau áp đặt sai lệch 75 Bảng 35 Thông số công trường hợp 86 \ DANH MỤC HÌNH ẢNH Hình 1.Một dạng tên lửa đơn giản Hình Nhà phát minh Robert H Goddard với tên lửa nhiên liệu lỏng giới Hình Tên lửa V-2 mở đường cho bùng nổ công nghệ tên lửa đạn đạo Hình Khởi động tên lửa Javelin 11 Hình Cấu tạo tên lửa Javelin 12 Hình Các hệ tọa độ chuyển động tên lửa 17 Hình Hệ trục tọa độ đại lượng chuyển động, khí động 18 Hình Ba phép quay theo ba góc ,  ,  ; chuyển động Yawing (a), chuyển động Pitching (b), Chuyển động Rolling (c) 19 Hình Các biến hình học thân 34 Hình 10 Các biến tổ hợp cánh 36 Hình 11 Kích thước tên lửa SA-2 47 Hình 12 Nhập liệu Missile datcom 49 Hình 13 Đầu Missile datcom 50 Hình 14 Cửa sổ giao diện Simulink 51 Hình 15 Thư viện simulink 52 Hình 16 Các khối Simulink 53 Hình 17 Thiết lập giải Simulink 55 Hình 18 Sơ đồ khối mơ ném xiên 56 Hình 19 Quỹ đạo ném xiên 58 Hình 20 Kích thước tên lửa Javelin 59 Hình 21 Kết hình học từ Datcom 60 Hình 22 Hệ số lực pháp tuyến theo vận tốc 61 Hình 23 Hệ số lực pháp tuyến theo góc 61 Hình 24 Hệ số lực dọc trục theo vận tốc 62 Hình 25 Hệ số lực dọc trục theo góc 62 Hình 26 Hệ số mơmen chúc ngóc theo vận tốc 63 Hình 27 Hệ số mơmen chúc ngóc theo góc 64 Hình 28 Mơ hình phi tuyến Simulink mơ tên lửa 68 Hình 29 Quỹ đạo bay trường hợp khơng lệch cánh lái 71 Hình 30 Tầm xa trường hợp không lệch cánh lái 71 Hình 31 Góc euler trường hợp khơng lệch cánh lái 72 Hình 32 Vận tốc tịnh tiến trường hợp không lệch cánh lái 72 Hình 33 Vận tốc góc trường hợp không lệch cánh lái 73 Hình 34 Quỹ đạo ngang trường hợp có sai lệch điều khiển 74 Hình 35 Góc euler trường hợp có sai lệch điều khiển 75 Hình 36 Vận tốc góc trường hợp có sai lệch điều khiển 75 Hình 37 Quỹ đạo trường hợp trọng tâm nằm sau tâm khí động 76 \ Hình 38 Góc pitch trường hợp trọng tâm nằm sau tâm khí động 77 Hình 39 Vận tốc tịnh tiến trường hợp trọng tâm nằm sau tâm khí động 77 Hình 40 Vận tốc góc trường hợp trọng tâm nằm sau tâm khí động 78 Hình 41 Ảnh hưởng gió ngang tới quỹ đạo bay 79 Hình 42 Ảnh hưởng gió dọc tới quỹ đạo bay 79 Hình 43 Quỹ đạo cơng đột 80 Hình 44 Mơ hình tính tốn điều khiển tên lửa 81 Hình 45 Góc cơng tên lửa 82 Hình 46 Quỹ đạo tên lửa tính tốn mơ hình 82 Hình 47 Góc lệch cánh lái q trình bay 83 Hình 48 Quỹ đạo theo phương thẳng đứng xOz thay đổi gió ngang 84 Hình 49.Quỹ đạo theo phương ngang xOy thay đổi gió ngang 84 Hình 50 Góc lệch cánh lái trường hợp có gió ngang 85 Hình 51 Quỹ đạo theo phương thẳng đứng xOz thay đổi gió dọc 85 Hình 52 Góc lệch cánh lái điều khiển trường hợp có gió dọc 86 Chương Mô quỹ đạo khảo sát ổn định Hình 50 Góc lệch cánh lái trường hợp có gió ngang b, Ảnh hưởng gió dọc Ta tiếp tục áp đặt sức gió m/s theo phương dọc (phương trục Ox, ngược hướng chuyển động tên lửa) lên tên lửa suốt trình bay, ta thu liệu quỹ đạo tên lửa thay đổi theo phương dọc hình 51 Quỹ đạo lệch khơng nhiều tên lửa cơng xác vào mục tiêu Hình 51 Quỹ đạo theo phương thẳng đứng xOz thay đổi gió dọc Giá trị góc lệch cánh điều khiển so sánh với trường hợp khơng có gió hình 52 Do gió ngược hướng chuyển động tạo lực cản nên thời gian bay tên lửa tăng lên 85 Chương Mô quỹ đạo khảo sát ổn định Hình 52 Góc lệch cánh lái điều khiển trường hợp có gió dọc Sau bảng so sánh thông số công quan trọng trường hợp nghiên cứu : Bảng 35 Thông số công trường hợp Thời gian bay(s) Vị trí cơng (m) Lệch phương ngang(m) Góc cơng (độ) Mục tiêu 13.81 2000.00 24-90 Khơng có gió 14.05 2000.01 33.04 Có gió ngang 14.09 2000.02 0.01 33.12 Có gió dọc 14.14 2000.02 33.23 Qua q trình tính tốn, ta thấy mơ hình đảm bảo tính xác có tác động ngoại lực gió Sự so sánh quỹ đạo khơng có gió có gió cho thấy sai lệch không đáng kể, chứng minh cho tính ổn định cao mơ hình tính tốn 86 KẾT LUẬN Với đề tài tốt nghiệp “Nghiên cứu, tính tốn ổn định điều khiển tên lửa sử dụng cơng cụ tính tốn số”, hướng dẫn TS Lưu Hồng Quân, em thực nội dung sau: - Tìm hiểu tên lửa công cụ mô Missile Datcom, Simulink: Qua phần ta biết bước thiết lập để chạy chương trình mơ - Xây dựng mơ hình tính tốn thơng số khí động tên lửa Javelin, lấy liệu khí động để mơ quỹ đạo bay - Dùng mơ hình sáu bậc tự mô quỹ đạo tên lửa Javelin trường hợp không lệch cánh lái trường hợp bay theo quỹ đạo thực tế Luận văn mô cho tên lửa tự theo quỹ đạo định trước công vào mục tiêu vị trí xác định, kết mơ cho thấy mơ hình có tính xác cao - Khảo sát tính ổn định tên lửa Javelin: Các kết mô thu cho thấy mơ hình sau chịu nhiễu động theo phương dọc phương ngang trở lại trạng thái ổn định sau nhiễu động, chứng minh tính ổn định cao mơ hình tên lửa Do khối lượng công việc nhiều thời gian thực LVTN có hạn, nên hướng nghiên cứu nhằm phát triển đề tài là: - Tính tốn thêm trường hợp nhiễu động khác để xem xét tính ổn định cách tồn diện - Xây dựng mơ hình thực tế đơn giản để kiểm chứng Cuối cùng, em xin gửi lời cảm ơn sâu sắc tới TS Lưu Hồng Quân – người trực tiếp hướng dẫn chúng em hoàn thành LVTN Em xin gửi lời cảm ơn chân thành tới Thầy, Cô giáo Bộ môn Hàng không giúp đỡ em suốt trình nghiên cứu Em xin chân thành cảm ơn 87 TÀI LIỆU THAM KHẢO [1] Isaac D Rose_Aerodynamic Modeling of an Unmanned Aerial Vehicle using a Computational Fluid Dynamics prediction code_ 2009 [2] Maurice, Andrew F_Aerodynamic performance predictions of a SA-2 Missile using Missile Datcom_2009 [3] Güneş Aydin_Aerodynamic Parameter Estimation of a Missile in Closed Loop Control and Validation with Flight Data_2012 [4] Lê Quang_Cơ học vật bay_2016 [5] Christopher Rosema, Joshua Doyle, Lamar Auman, and Mark Underwood _ MISSILE DATCOM User’s Manual - 2011 Revision_2011 [6] Headquarters, Department Of The Army_ Javelin— Close Combat Missile System, Medium_2008 [7] Brian L Stevens, Frank L Lewis, Eric N Johnson_ Aircraft Control and Simulation_2016 3, [8] David Qi Zhang_ Design and Analysis of the Two-Stage FGM-148 Javelin Anti-Tank Missile_2012 [9] John Harris, Nathan Slegers_Performance of a fire-and-forget anti-tank missile with a damaged wing_2009 [10] Arda Aksu_Aerodynamic Parameter Estimation of a Missile_2013 88 PHỤ LỤC Phụ lục Hàm tính tốn lực mơmen (forces_moments.m) function output_args = forces_moments( input_args ) x_e = input_args(1); y_e = input_args(2); z_e = input_args(3); phi = input_args(4); theta = input_args(5); psi = input_args(6); u = input_args(7); v = input_args(8); w = input_args(9); p = input_args(10); q = input_args(11); r = input_args(12); % delta_right_ail = input_args(13); % delta_left_ail = input_args(14); % delta_rud = input_args(15); % thrust = input_args(16); delta_ail = input_args(13); delta_elv = input_args(14); delta_rud = input_args(15); thrust = input_args(16); delta_right_ail = delta_elv - delta_ail; delta_left_ail = delta_elv + delta_ail; if delta_right_ail < -45 delta_right_ail = -45; else if delta_right_ail > 45 delta_right_ail = 45; end end if delta_left_ail < -45 delta_left_ail = -45; else if delta_left_ail > 45 delta_left_ail = 45; end end if delta_rud < -25 delta_rud = -25; else if delta_rud > 25 delta_rud = 25; end end % if thrust < % thrust = 0; % else if thrust > 25*4.448222 %4.448222 lbf = newtons % thrust = 25*4.448222; %4.448222 lbf = newtons % end % end %delta_rud=0; 89 % delta_ail = input_args(13); % delta_ele = input_args(14); % delta_rud = input_args(15); % delta_thrust= input_args(16); %************************************ Vt = sqrt(u^2+v^2+w^2); if u~=0 alpha = atan2(w,u); beta = asin(v/Vt); else alpha = pi/2; beta = 0; end %create thrust force vector F_thrust_b = [thrust 0]'; %altitude = -H = -z_e; deltadeg = [delta_right_ail,delta_left_ail,delta_rud,delta_rud,0,0]; omega = [p q r]'; rho = 1.229; %kg/m^3 1.229 bref = 0.13; %m Sref = 0.013; %m^2 cbar = 0.13; %m %this has been changed because the for005 file is setup in VINF m/s sos = 340.29;% 340.29 m/s at sea level alphadeg = alpha*180/pi; betadeg = beta*180/pi; H=-z_e; %aerodynamic forces [tau, f] = datcomderive(alphadeg, betadeg, H, deltadeg, Vt, omega,bref,cbar,Sref,sos,rho,delta_elv,delta_rud); F = f + F_thrust_b; T = tau; output_args = [F(1) F(2) F(3) T(1) T(2) T(3)]; return 90 Phụ lục Hàm tính tốn phụ forces_moments.m (datcomderive.m) function [tau, f] = datcomderive(alphadeg, betadeg, H, deltadeg, Vt, omega,bref,cbar,Sref,sos,rho,delta_elv,delta_rud) % mach number mach = Vt / sos; % dynamic pressure qbar = 0.5 * rho * Vt^2; % Scale dynamic derivatives lat_scale = 0.5 * (bref) / Vt; long_scale = 0.5 * (cbar) / Vt; omega_scale = omega.* [lat_scale; long_scale; lat_scale]; % calculate coefficients % alpha and mach effects for nominal [DepBaseIncrements, Derivatives_Stab] = impdatcom1(alphadeg,mach); C_norm = DepBaseIncrements; %components: [N, M, A, Y, ln, ll] % Scaling the Stability derivative coefficients C_scale = Derivatives_Stab * omega_scale([2 2 3 1 1])'; %component: [CNQ, CMQ, CAQ, CYR, ClnR, CllR, CYP, ClnP, CllP] % Form base contributions to moments and forces C_BAE = C_norm + C_scale(1:6) + [zeros(1,3), C_scale(7:9)]; %components: [N, M, A, Y, ln, ll] % delta contributions to moments and forces DepDeltaIncrements=[0.00 0.000 0.000 0 0.000]; C_delta = DepDeltaIncrements; %components: [N, M, A, Y, ln, ll] % % output moments and forces %MFoutput = qbar * (Sref) * [p.bref; p.cbar; p.bref; -1; 1; -1] * (C_BAE([6 1])' + C_delta([6 1])'); %components: [L, M, N, X, Y, Z] tau = qbar * (Sref) * [bref; cbar; bref] * (C_BAE([6 5])' + C_delta([6 5])'); %components: [L, M, N] f = qbar * (Sref) * [-1; 1; -1] * (C_BAE([3 1])' + C_delta([3 1])'); %components: [X, Y, Z] 91 Phụ lục Hàm S-function tính tốn phương trình chuyển động function [sys,x0,str,ts] = equation(t,x,u,flag,g,x0) switch flag, %%%%%%%%%%%%%%%%%% % Initialization % %%%%%%%%%%%%%%%%%% case 0, % % call simsizes for a sizes structure, fill it in and convert it to a % sizes array % % Note that in this example, the values are hard coded This is not a % recommended practice as the characteristics of the block are typically % defined by the S-function parameters % sizes = simsizes; sizes.NumContStates = 12; sizes.NumDiscStates = 0; sizes.NumOutputs = 12; sizes.NumInputs = 6; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; % at least one sample time is needed sys = simsizes(sizes); % % str is always an empty matrix % str = []; % % initialize the array of sample times % ts = [0 0]; % % initialize the initial conditions % x0=[0 -1 0.641 13 0 0 0]'; Xe = x0(1); %units of linear distance Ye = x0(2); %units of linear distance Ze = x0(3); %units of linear distance Phi = x0(4); %radians Theta = x0(5); %radians Psi = x0(6); %radians U = x0(7); %units of linear distance per second V = x0(8); %units of linear distance per second W = x0(9); %units of linear distance per second P = x0(10); %units of radians per second Q = x0(11); %units of radians per second R = x0(12); %units of radians per second nav_dot=[0 0]'; euler_dot=[0 0]'; vel_dot=[0 0]'; omega_dot=[0 0]'; % end mdlInitializeSizes %%%%%%%%%%%%%%% 92 % Derivatives % %%%%%%%%%%%%%%% case 1, %Inputs Fx = u(1); Fy = u(2); Fz = u(3); l = u(4); m = u(5); n = u(6); %states Xe = x(1); %units of linear distance Ye = x(2); %units of linear distance Ze = x(3); %units of linear distance Phi = x(4); %radians Theta = x(5); %radians Psi = x(6); %radians U = x(7); %units of linear distance per second V = x(8); %units of linear distance per second W = x(9); %units of linear distance per second P = x(10); %units of radians per second Q = x(11); %units of radians per second R = x(12); %units of radians per second v_cm_e = [U V W]'; %Velocity components Force = [Fx Fy Fz]'; %Foce Input Moment = [l m n]'; %Moment Input omega = [P Q R]'; %Body Rates %Inertial Matrix %From Stevens and Lewis pg.45 equation 1.5-7 %Inertial Matrix if t2.4&&t