Ứng dụng mạng nơron nhận dạng các tham số khí động kênh độ cao nhằm nâng cao hiệu qủa thiết kế thiết bị bay

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ - NGUYỄN ĐỨC THÀNH ỨNG DỤNG MẠNG NƠRON NHẬN DẠNG CÁC THAM SỐ KHÍ ĐỘNG KÊNH ĐỘ CAO NHẰM NÂNG CAO HIỆU QUẢ THIẾT KẾ THIẾT BỊ BAY LUẬN ÁN TIẾN SĨ KỸ THUẬT Hà Nội – 2021 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHỊNG VIỆN KHOA HỌC VÀ CƠNG NGHỆ QUÂN SỰ - NGUYỄN ĐỨC THÀNH ỨNG DỤNG MẠNG NƠRON NHẬN DẠNG CÁC THAM SỐ KHÍ ĐỘNG KÊNH ĐỘ CAO NHẰM NÂNG CAO HIỆU QUẢ THIẾT KẾ THIẾT BỊ BAY Chuyên ngành: Kỹ thuật điều khiển tự động hóa Mã số: 9.52.02.16 LUẬN ÁN TIẾN SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: TS Trương Đăng Khoa TS Hoàng Minh Đắc Hà Nội - 2021 i LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Những nội dung, số liệu kết trình bày luận án hồn tồn trung thực chưa có tác giả cơng bố cơng trình khác Các liệu tham khảo trích dẫn đầy đủ Tác giả luận án Nguyễn Đức Thành ii LỜI CẢM ƠN Cơng trình nghiên cứu thực Viện Tên lửa, Viện Tự động hoá kỹ thuật quân thuộc Viện Khoa học Công nghệ quân - Bộ Quốc phòng Tác giả xin bày tỏ biết ơn sâu sắc tới TS Trương Đăng Khoa TS Hoàng Minh Đắc định hướng nghiên cứu tận tình hướng dẫn, giúp đỡ suốt trình thực luận án Tác giả luận án xin chân thành cảm ơn thủ trưởng Viện Khoa học Cơng nghệ qn sự, Phịng Đào tạo/ Viện Khoa học Công nghệ quân sự, Viện Tên lửa, Viện Tự động hoá kỹ thuật quân Viện Khoa học Công nghệ quân sự, khoa KTĐK Học viện KTQS đồng nghiệp động viên, quan tâm giúp đỡ để tác giả hoàn thành luận án Xin chân thành cám ơn Thầy giáo, nhà Khoa học gia đình, người thân bạn bè quan tâm giúp đỡ, đóng góp nhiều ý kiến quý báu, cổ vũ động viên tác giả hoàn thành cơng trình khoa học NCS Nguyễn Đức Thành iii MỤC LỤC LỜI CAM ĐOAN i LỜI CẢM ƠN ii MỤC LỤC iii DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT vi DANH MỤC CÁC BẢNG x DANH MỤC CÁC HÌNH VẼ xi MỞ ĐẦU CHƯƠNG I: TỔNG QUAN VỀ NHẬN DẠNG CÁC HỆ SỐ KHÍ ĐỘNG CỦA MÁY BAY 1.1 Nhiệm vụ nhận dạng đạo hàm hệ số khí động quy trình thiết kế thiết bị bay 1.1.1 Các giai đoạn thiết kế thiết bị bay 1.1.2 Mơ hình khí động thiết bị bay 1.1.3 Vai trị nhận dạng đặc tính khí động 16 1.2 Nhận dạng đạo hàm hệ số khí động từ liệu bay 17 1.2.1 Mơ hình động học chuyển động máy bay 18 1.2.2 Thử nghiệm bay, thu nhận liệu 19 1.2.3 Ước lượng tham số khí động xác định trạng thái 21 1.2.4 Xác nhận mơ hình 24 1.3 Tình hình nghiên cứu nước 25 1.4 Tình hình nghiên cứu nước 33 1.5 Đặt vấn đề nghiên cứu 35 1.5.1 Những vấn đề tồn 35 1.5.2 Xây dựng hướng nghiên cứu luận án 36 1.6 Kết luận chương 37 iv CHƯƠNG 2: XÂY DỰNG MƠ HÌNH ĐỘNG HỌC CHO MỘT LỚP MÁY BAY CÁNH BẰNG 38 2.1 Mơ hình động học máy bay 38 2.1.1 Các hệ tọa độ sử dụng mô tả chuyển động máy bay 38 2.1.2 Các quy ước chiều dấu tác động điều khiển 41 2.1.3 Mơ hình động học phi tuyến 43 2.2 Mơ hình động học máy bay kênh độ cao 48 2.2.1 Mơ hình trạng thái chuyển động phi tuyến 48 2.2.2 Mơ hình trạng thái chuyển động tuyến tính kênh độ cao 49 2.3 Mơ hình động học dùng cho nhận dạng đạo hàm hệ số khí động thiết bị bay ứng dụng mạng nơron nhân tạo 52 2.3.1 Mơ hình động học thiết bị bay dùng cho mạng nơron nhân tạo 52 2.3.2 Mạng nơron đột biến 54 2.4 Kết luận chương 63 CHƯƠNG 3: XÂY DỰNG THUẬT TOÁN NHẬN DẠNG MỘT SỐ ĐẠO HÀM HỆ SỐ KHÍ ĐỘNG CỦA MÁY BAY 64 3.1 Thuật toán nhận dạng đạo hàm hệ số khí động máy bay theo phương pháp truyền thống 64 3.1.1 Thuật toán nhận dạng đạo hàm hệ số khí động máy bay theo phương pháp hồi quy tuyến tính 64 3.1.2 Thuật toán nhận dạng đạo hàm hệ số khí động máy bay theo phương pháp sai số đầu 68 3.2 Xây dựng thuật tốn nhận dạng đạo hàm hệ số khí động máy bay sử dụng mạng nơron nhân tạo 80 3.2.1 Xây dựng thuật toán nhận dạng đạo hàm hệ số khí động máy bay sử dụng mạng RBF 80 v 3.2.2 Xây dựng thuật toán nhận dạng đạo hàm hệ số khí động máy bay ứng dụng mạng nơron đột biến 83 3.3 Kết luận chương 95 CHƯƠNG 4: MƠ PHỎNG VÀ ĐÁNH GIÁ CÁC THUẬT TỐN NHẬN DẠNG CÁC ĐẠO HÀM HỆ SỐ KHÍ ĐỘNG CỦA MÁY BAY 96 4.1 Mô đánh giá thuật toán nhận dạng đạo hàm hệ số khí động máy bay sử dụng phương pháp truyền thống 97 4.1.1 Mô đánh giá thuật toán nhận dạng đạo hàm hệ số khí động theo phương pháp LR 97 4.1.2 Mô đánh giá thuật toán nhận dạng đạo hàm hệ số khí động theo phương pháp OEM 100 4.2 Mô đánh giá thuật tốn nhận dạng đạo hàm hệ số khí động ứng dụng mạng nơron nhân tạo 103 4.2.1 Mô đánh giá thuật toán nhận dạng đạo hàm hệ số khí động ứng dụng mạng RBF 104 4.2.2 Mô đánh giá thuật toán nhận dạng đạo hàm hệ số khí động ứng dụng mạng nơron đột biến 109 4.3 Kết luận chương 116 KẾT LUẬN VÀ KIẾN NGHỊ 118 DANH MỤC CÁC CƠNG TRÌNH KHOA HỌC ĐÃ CƠNG BỐ 120 TÀI LIỆU THAM KHẢO 121 PHỤ LỤC a vi DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT Danh mục ký hiệu ,  Góc cơng, góc trượt   0 Góc công đảm bảo cho máy bay chế độ bay ,  ,  Góc gật, góc liệng, góc hướng   δe , δr , δa Góc lệch cánh lái độ cao, cánh lái hướng, cánh lái liệng   δe0 Góc lệch cánh lái độ cao đảm bảo máy bay chế độ bay  Mật độ khơng khí  kg / m3   Sai lệch chuẩn  Véc tơ tham số mơ hình  x ,  y ,  z Các thành phần tốc độ góc hệ tọa độ liên kết  /s    A, B, C, D Ma trận hệ thống tuyến tính ax , ay , az Các thành phần gia tốc hệ tọa độ liên kết  m / s  bA , bAe Cung khí động trung bình cánh nâng, cánh lái độ cao  m  CL , CD Hệ số lực nâng hệ số lực cản hệ tọa độ tốc độ CD , CL , m y Đạo hàm hệ số lực cản, hệ số lực nâng, hệ số mơ men gật theo góc công 1 /    CD , CL , my Đạo hàm hệ số lực cản, hệ số lực nâng, hệ số mô men gật theo y y z tốc độ góc gật  s /  CD , CL , my e e e Đạo hàm hệ số lực cản, hệ số lực nâng, hệ số mơ men gật theo góc lệch cánh lái độ cao 1 /  Cx , C y , Cz Các thành phần hệ số lực khí động hệ tọa độ liên kết vii F Véc tơ lực tác động lên thiết bị bay  N  FA Véc tơ lực khí động tác động lên thiết bị bay  N  FG Véc tơ trọng lực tác động lên thiết bị bay  N  FP Véc tơ lực đẩy động tác động lên thiết bị bay  N  g Gia tốc trọng trường  m / s  H Độ cao thiết bị bay  m  I Ma trận mô men quán tính  kg.m2  Ix, I y , Iz Các mơ men qn tính hệ tọa độ liên kết  kg.m2  l , le Sải cánh nâng, cánh lái độ cao  m  L, D Lực nâng lực cản hệ tọa độ tốc độ  N  m Khối lượng thiết bị bay  kg  M Véc tơ mô men tác động lên thiết bị bay  Nm  MA Véc tơ mô men khí động tác động lên thiết bị bay  Nm  MP Véc tơ mô men lực đẩy động tác động lên thiết bị bay  Nm  Mx, M y, Mz Các thành phần mơ men khí động hệ tọa độ liên kết  Nm  mx , my , mz Các thành phần hệ số mơ men khí động hệ tọa độ liên kết N Số điểm liệu bay ghi nhận Oxyz Hệ tọa độ liên kết Oxa ya za Hệ tọa độ tốc độ OxE y E z E Hệ tọa độ mặt đất Oxg yg zg Hệ tọa độ mặt đất di động P Lực đẩy động  N  viii q Áp suất khí động  N / m3  R Ma trận hiệp phương sai sai số S Diện tích đặc trưng cánh  m  x Véc tơ tham số trạng thái X, Y, Z Các thành phần lực khí động hệ tọa độ liên kết  N  y, z Véc tơ tham số đầu mơ hình tập liệu bay V Không tốc thiết bị bay  m / s  Vx , Vy , Vz Các thành phần tốc độ hệ tọa độ liên kết  m / s  Danh mục chữ viết tắt 2.1 Tiếng Anh ANN Artificial Neural Network Mạng nơron nhân tạo ACD Aerodynamic Coefficient Đạo hàm hệ số khí động Derivative BP Back Propagation Lan truyền ngược CFD Computational Fluid Tính tốn động học chất lỏng Dynamics EKF Extended Kalman Filter Bộ lọc Kalman mở rộng FEM Filter Error Method Phương pháp sai số lọc GN Gauss – Newton Thuật tốn Gauss – Newton IF Integrate and Fire Tích lũy kích hoạt LIF Leaky Integrate and Fire Tích lũy kích hoạt có tổn thất LS Least Squares Bình phương nhỏ LR Linear Regression Hồi quy tuyến tính f C.2 Giới hạn Cramer-Rao Cực tiểu hóa ma trận hiệp phương sai tham số ước tính cho thuật tốn ước lượng khơng chệch, gọi giới hạn Cramer-Rao, với nghịch đảo ma trận thơng tin Fisher Điều có nghĩa ma trận hiệp phương sai tham số ước lượng thỏa mãn bất đẳng thức: Cov(ˆ)  M 1 (P.9) Để rút biểu thức, xét với thuật tốn ước lượng khơng chệch:     ˆ    p(z / θ)dz  E ˆ    (P.10)  Lấy vi phân theo θ ta được:     T  p(z / θ )  dz  ˆ           Ip(z / θ )dz     (P.11)  I ma trận xác định,  p(z / θ )dz  thì:     T  p(z / θ )  ˆ    dz  I     (P.12) Giới thiệu ký hiệu toán tử kỳ vọng sử dụng phương trình (P.8)   p(z / θ)   E  ˆ     I        (P.13) Bổ đề: Đặt X Y hai véc tơ ngẫu nhiên có chiều, đó: E  XX T   E  XY T   E YY T   E YX T  1  Áp dụng bổ đề với: X = ˆ    Y= (P.14) lnp(z / θ )  Từ phương trình (P.14) cho kết quả: 1 T   lnp(z / θ ) lnp(z / θ )   E  ˆ   ˆ     I  E    I     T    Kết hợp với (P.12) ta được:        T Cov ˆ   ˆ   ˆ     M 1   (P.15) (P.16) g PHỤ LỤC D: TÍNH ĐỘ NHẠY ĐẦU RA Độ nhạy đầu xác định cách lấy đạo hàm riêng phương trình đầu (3.26 – b) theo tham số  j , j  1,2, , np : D   y x C   x u , j  1,2, , np  C     j  j  j  j (P.17) Trạng thái x tính từ phương trình trạng thái (3.26 - a): x = A  x + B   u , x(0) = x0 Độ nhạy trạng thái (P.18) x , j  1,2, , np tính cách giải  j phương trình độ nhạy trạng thái, thu cách đạo hàm phương trình trạng thái (P.18) tham số ĐHHSKĐ: B   x(0) d  x  x A  x u,   , j  1,2, , np (P.19)    A   j  j  j  j dt   j  Để giải phương trình vi phân (P.17), (P.18) theo phương pháp số, cơng thức rời rạc sử dụng với ma trận chuyển trạng thái xác định: x (i )  x (i  1)    u(i )  u(i  1)  , x (0)  x0 (P.20) Phương trình xác định độ nhạy trạng thái: A    x (i)  x (i  1) x (i) x (i  1) 1   + A   (e At - I)  j  j  j  A   B     u(i )  u(i  1) x(0) 0 B    ,    j   j   j đó:   e A t  t  1 A  A  t d B   =  A   e   - I) B      e    (P.21) (P.22) h PHỤ LỤC E: CÁC CHƯƠNG TRÌNH NHẬN DẠNG CÁC ĐẠO HÀM HỆ SỐ KHÍ ĐỘNG E.1 Chương trình matlab nhận dạng đạo hàm hệ số khí động từ liệu thu thập từ chuyến bay thực tế theo phương pháp LR clear all; clf %#### NHAP DU LIEU CHUYEN BAY BAY TU EXCEL ##### Data = xlsread('D:\Thanh_NCS\DLMB_Su-D\Dulieu_1.xls'); tocdo= data(:,3); % du lieu toc a_x= data(:,5); % du lieu gia toc thang % du lieu gia toc dung a_z= data(:,6); alpha = data(:,7); % du lieu goc alpha goc_gat = data(:,10); % du lieu goc gat omega_y= data(:,18); % du lieu toc goc gat % du lieu goc lech canh lai cao delta_e= data(:,17); %#### CAC THAM SO DAC TRUNG VÀ DIEU KIEN BAY ##### % Cung dong canh nang b_A= 4.6; l= 14.7; % sai canh nang % luc day dong co P=14500; S= 62.0; % dien tich dac trung canh; m= 24900; % luong may bay; % toc tham chieu V0=850km/h ; Vo=236.11; ro= 0.67; % mat khong khi; q= ro*Vo^2/2; % ap suat dong; g=9.806; % gia toc truong Ix= 12350; % mo men quan tinh trục Ox; % mo men quan tinh trục Oy; Iy= 62010; Iz= 27600; % mo men quan tinh trục Oz; % mo men quan tinh; Ixz= 756; % XAC DINH HE SO LUC NANG VA LUC CAN TU DU LIEU BAY C_z= m*a_z/(q*S); % cong thuc (1.12) % cong thuc (1.12) C_x= (m*a_x-P)/(q*S); C_L= -C_z.*cos(alpha)+C_x.*sin(alpha); % Tính C_L theo(1.12) C_D= -C_x.*cos(alpha)-C_z.*sin(alpha); % Tính C_D theo(1.12) %### TINH HE SO MÔ MEN GAT TU DU LIEU BAY #### N=length(goc_gat); giatoc_gat= diff(omega_y); % Tính dao ham tocdo_goc gat m_y= Iy.*giatoc_gat1/(q*S*b_A); % Cong thuc(1.12) %#### NHAN DANG CAC DHHSKD (3.7) ##### % Tinh ma tran hoi quy X u1=alpha; u2=b_A*omega_y/(2*Vo); u3=delta_e; % tao vec to cot =1 u0_VT=ones(N,1); X_MT=[u0_VT u1 u2 u3]; % ma tran hoi quy(3.9) % Nhan dang he so luc can CD ct(3.7_a): y1= C_D; % Du lieu CD i % Nhan dang CD theo(3.6) CD_TS=(inv(X_MT'*X_MT))*X_MT'*y1; CD=CD_TS(1,1)+CD_TS(2,1)*u1+CD_TS(3,1)*u2+CD_TS(4,1)*u3; % Tinh CL theo (3.7_b) % Du lieu CL y2= C_L'; CL_TS=(inv(X_MT'*X_MT))*X_MT'*y2'; % Nhan dang CL theo(3.6) CL=CL_TS(1,1)+CL_TS(2,1)*u1+CL_TS(3,1)*u2+CL_TS(4,1)*u3; % Tinh my theo (3.7_c) y3= m_y'; % m_y du lieu my_TS= (inv(X_MT'*X_MT))*X_MT'*y3'; % Nhan dang CL (3.6) my=my_TS(1,1)+my_TS(2,1)*u1+my_TS(3,1)*u2+my_TS(4,1)*u3; %VE DO THI KET QUA NHAN DANG % thi so sanh giua du lieu y1 va dau mo hinh CD subplot(3,1,1); plot(1:N,y1,'b',1:N,CD, 'r'); xlabel('t[s]]'); ylabel('HE SO C_D'); grid on % thi so sanh giua du lieu y2 va dau mo hinh CL subplot(3,1,2); plot(1:N,y2,'b',1:N,CL, 'r'); xlabel('t[s]'); ylabel('HE SO C_L'); grid on % thi so sanh giua du lieu y3 va dau mo hinh my subplot(3,1,3); plot(1:N,y3,'b',1:N,my, 'r'); xlabel('t[s]'); ylabel('HE SO m_y'); grid on E.2 Chương trình Matlab nhận dạng ĐHHSKĐ từ liệu bay theo OEM clear all; clf %#### NHAP DU LIEU CHUYEN BAY BAY TU EXCEL ##### data= xls('D:\Thanh_NCS\DLMB_Su-D\bay_bang_1.xls'); docao = data(:,2); % du lieu cao bay tocdo= data(:,3); % du lieu toc ro= data(:,20); % mat không (thay doi theo H) goc_gat = data(:,10); % du lieu goc gat % du lieu goc tan cong alpha = data(:,7); a_z= data(:,4); % du lieu gia toc dung a_x= data(:,5); % du lieu gia toc thang % du lieu goc lech canh lai cao delta_e= data(:,17); omega_y= data(:,18); % du lieu toc goc gat % du lieu goc lech canh lai cao TB delta_TB= data(:,19); %CAC THAM SO DAC TRUNG CUA MB VÀ DIEU KIEN BAY THAM CHIEU b_A= 4.645 ; % Cung dong canh nang b_A_delta=1.25; % Cung dong cánh lái cao l= 14.7; % sai canh nang % luc day dong co P=14500; S= 62.0; % dien tich dac trung canh S_delta= 12.24; % dien tich canh lai cao tham chieu j % luong may bay m= 24900 ; Vo=236.11; % toc MB tham chieu; q= ro*Vo^2/2; % ap suat dong % gia toc truong g=9.806; Ix= 12350; % mo men quan tinh theo truc Ox % mo men quan tinh theo truc Oy Iy= 62010; Iz= 27600; % mo men quan tinh theo truc Oz Ixz= 756; % mo men quan tinh %XAC DINH HE SO LUC NANG VA LUC CAN TU DU LIEU BAY C_z= m*a_z/(q*S); % cong thuc (1.12) % cong thuc (1.12) C_x= (m*a_x-P)/(q*S); C_L= -C_z.*cos(alpha)+C_x.*sin(alpha); % Tính C_L (1.12) C_D= -C_x.*cos(alpha)-C_z.*sin(alpha); % Tính C_D (1.12) %### TINH HE SO MƠ MEN GAT TU DU LIEU BAY #### N=length(goc_gat); giatoc_gat= diff(omega_y); % Tính dao ham tocdo_goc gat giatoc_gat1= [giatoc_gat(1:N-1);giatoc_gat(N-2)]; m_y= Iy.*giatoc_gat1/(q*S*b_A); % Cong thuc (1.12) % ### TINH CAC DAN XUAT THEO (2.38) ##### L_alpha=q*S/(m*Vo); % dan suat luc nang theo α L_omega=q*S*b_A/(2*m*Vo*Vo); % dan suat luc nang theo ωy L_delta=q*S_delta/(m*Vo); % dan suat luc nang theo δe M_alpha=q*S*b_A/Iy; % dan suat mô men gat theo α M_omega=q*S*b_A*b_A/(2*Vo*Iy); % dan suat My theo ωy M_delta=q*S_delta*b_A_delta/Iy; % dan suat My theo δe %**** TINH CAC DHHSKD THEO PP OEM **** tol= 1e-3; % chinh xac tham so % so luong buoc lap cuc dai maxstep = 50; teta0=[1 10 0.1 -0.1 -10 -0.1 0 0]'; % gia tri dau teta teta_old= teta0; J_old = 0; for k=1:maxstep %#### XAC DINH CAC MA TRAN A, B, C, D ##### % Tinh ma tran A A11=-L_alpha* teta_old(1); A12= L_omega* teta_old(2); A21= M_alpha* teta_old(4); A22= M_omega* teta_old(5); A=[A11, (1-A12);A21, A22]; % ma tran A % Tinh ma tran B B11= -L_delta*teta_old(3); B12= teta0(7); B21= M_delta*teta_old(6); B22= teta_old(8); B= [B11 B12;B21 B22]; % ma tran B % Tinh ma tran C C11=1; C12=0; C21=0; C22= 1; C31= -(Vo*L_alpha*teta_old(1))/g; C32= -(Vo*L_omega*teta_old(2))/g; % C=[1 0; 1; C31 C32] C=[C11 C12; C21 C22; C31 C32]; % Tinh ma tran D D11=0; D12=0; D21=0; D22=0; k D31=-(Vo*L_delta*teta_old(3))/g; D32=teta_old(9); D=[D11 D12; D21 D22; D31 D32]; % D=[0 0;0 0;D31 D32] % XAC DINH DAO HAM CAC A, B, C, D THEO THAM SO TETA % tinh dao ham ma tran A theo teta (dA_dteta) % dao ham A theo CL_α d_A_dteta1= [-L_alpha 0; 0 ]; d_A_dteta2= [0 -L_omega; 0 ]; % dao ham A theo CL_ωy d_A_dteta3= [0 0; 0]; % dao ham A theo my_α d_A_dteta4= [0 0;M_alpha 0]; d_A_dteta5= [0 0; M_omega]; % dao ham A theo my_ωy d_A_dteta6= [0 0; 0]; d_A_dteta7= [0 0; 0]; d_A_dteta8= [0 0; 0]; d_A_dteta9= [0 0; 0]; % Tinh dao ham ma tran B theo teta (d_B_dteta) d_B_dteta1= [0 0; 0 ]; d_B_dteta2= [0 0; 0 ]; d_B_dteta3= [-L_delta 0; 0 ]; % dao ham B theo CL_δe d_B_dteta4= [0 0; 0 ]; d_B_dteta5= [0 0; 0 ]; % dao ham B theo my_δe d_B_dteta6= [0 0; M_delta ]; d_B_dteta7= [0 1;0 0]; % dao ham B theo teta7 % dao ham B theo teta8 d_B_dteta8= [0 0;0 1]; d_B_dteta9= [0 0; 0 ]; % tinh dao ham ma tran C theo teta(dC_dteta) dC_dteta1= [0 0;0 0;-(Vo*L_alpha)/g 0]; % dao ham C theo CL_α dC_dteta2= [0 0;0 0; -(Vo*L_omega)/g];%dao ham C theo CL_ω dC_dteta3= [0 0;0 0; 0]; dC_dteta4= [0 0;0 0; 0]; dC_dteta5= [0 0;0 0; 0]; dC_dteta6= [0 0;0 0; 0]; dC_dteta7= [0 0;0 0; 0]; dC_dteta8= [0 0;0 0; 0]; dC_dteta9= [0 0;0 0; 0]; % tinh dao ham ma tran D theo teta (dD_dteta) d_D_dteta1= [0 0;0 0;0 0]; d_D_dteta2= [0 0;0 0;0 0]; d_D_dteta3= [0 0;0 0; -(Vo*L_delta)/g 0]; d_D_dteta4= [0 0;0 0;0 0]; d_D_dteta5= [0 0;0 0;0 0]; d_D_dteta6= [0 0;0 0;0 0]; d_D_dteta7= [0 0;0 0;0 0]; d_D_dteta8= [0 0;0 0;0 0]; % dao ham ma tran D theo teta9 d_D_dteta9= [0 0;0 0;0 1]; % TINH TRANG THAI x(i)THEO CONG THUC(P.20) % Tinh CT (P.22) PHI=expm(A/50); % tinh PHI theo(P.22)(ham e_mu; T=1/50 (s)) % MT nghich dao A_1=inv(A); l % MT duong cheo I=eye(2); GO= (A_1*(PHI-I))*B; % tinh MT GO(P.22) % Tinh u_i va u_TB=(ui1+ui2)/2 u_i1= delta_e'; % tinh u_TB1= (delta(i)+(delta(i+1))/2 u_TB1= delta_TB'; u_i2=[ones(1,600)]; %tao vec to 600 phan tu co gia tri u_i=[u_i1;u_i2] ; % vec to dau vao dieu khien u_TB=[u_TB1; u_i2]; % gia tri vec to dau vao dieu khien z_i=[alpha'; omega_y'; a_z']; % Du lieu dau x10=sum(alpha)/N; % gia tri trang thai dau cua alpha x20=sum(omega_y)/N; % gia tri trang thai dau cua omega x0=[x10;x20]; % gia tri vec to trang thai dau for i=1:600 x= PHI*x0 + GO*u_TB(:,i); % cong thuc(P.20) y= C*x+ D*u_i(:,i); % Tinh gia tri dau theo (3.26_b) % Tinh x_iTB=(x(i)+x(i+1))/2 x_TB_i=(x0+x)/2 ; x_TB(:,i)=x_TB_i; x_i(:,i) = x; y_i(:,i) = y; x0=x; % x= [alpha(i);omega_y(i)]; end % Tinh sai lech E_i (3.27)va phuong sai R CT (3.29) % sai so giua dau mo hình va du lieu E_i=z_i-y_i; R1=(E_i*E_i')/N; % ma tran hiep phuong sai sai so %#### TINH DO NHAY TRANG THAI (P.21) ##### dx_dteta0=zeros(2,9); % gia tri dao ham trang thai dau dx_i_dteta0= dx_dteta; M_T=zeros(9); g_T=zeros(9,1); J_T=0; for i=1:600 % Tinh dA_dteta_xTB phan tu thu hai cua ct(P.21) % TP thu trong(P.21) dx_i_dteta_j_1= PHI* dx_i_dteta0; % Tinh ma tran [dA_dteta_xTB] phan tu thu hai (P.21) dA1_xTB= d_A_dteta1* x_TB(:,i); dA2_xTB= d_A_dteta2* x_TB(:,i); dA3_xTB= d_A_dteta3* x_TB(:,i); dA4_xTB= d_A_dteta4* x_TB(:,i); dA5_xTB= d_A_dteta5* x_TB(:,i); dA6_xTB= d_A_dteta6* x_TB(:,i); dA7_xTB= d_A_dteta7* x_TB(:,i); dA8_xTB= d_A_dteta8* x_TB(:,i); dA9_xTB= d_A_dteta9* x_TB(:,i); dA_dteta_xTB=[dA1_xTB dA2_xTB dA3_xTB dA4_xTB dA5_xTB dA6_xTB dA7_xTB dA8_xTB dA9_xTB]; dx_i_dteta_j_2= PHI*dA_dteta_xTB; % TP thu ct (P.21) %### Tinh phan tu thu ba cua ct (P.21) #### % Tich cac ma tran (d_A_dteta*B + d_B_dteta)cong thuc(P.21) m D_AB1=d_A_dteta1*B+d_B_dteta1; D_AB2=d_A_dteta2*B+d_B_dteta2; D_AB3=d_A_dteta3*B+d_B_dteta3; D_AB4=d_A_dteta4*B+d_B_dteta4; D_AB5=d_A_dteta5*B+d_B_dteta5; D_AB6=d_A_dteta6*B+d_B_dteta6; D_AB7=d_A_dteta7*B+d_B_dteta7; D_AB8=d_A_dteta8*B+d_B_dteta8; D_AB9=d_A_dteta9*B+d_B_dteta9; % Tinh TP thu cong thuc(P.21) dAB1= D_AB1*u_TB(:,i); dAB2= D_AB2*u_TB(:,i); dAB3= D_AB3*u_TB(:,i); dAB4= D_AB4*u_TB(:,i); dAB5= D_AB5*u_TB(:,i); dAB6= D_AB6*u_TB(:,i); dAB7= D_AB7*u_TB(:,i); dAB8= D_AB8*u_TB(:,i); dAB9= D_AB9*u_TB(:,i); dAB_dteta_uTB=[ dAB1 dAB2 dAB3 dAB4 dAB5 dAB6 dAB7 dAB8 dAB9]; dx_i_dteta_j_3=(A_1*(PHI-I))*dAB_dteta_uTB; % TP thu 3(P.21) % Tinh nhay trang thai theo cong thuc (P.21) dx_i_dteta_j= dx_i_dteta_j_1+dx_i_dteta_j_2+ dx_i_dteta_j_3; dx_i_dteta_j_N(:,:,i)= dx_i_dteta_j; % CT(P.21)ca bo du lieu dx_i_dteta0=dx_i_dteta_j; % TINH DO NHAY DAU RA THEO(P.17) % Tinh TP thu nhat (P.17) dy_i_dteta_j_1= C*dx_i_dteta_j; % TP thu cua(P.17) % Tinh TP thu hai (P.17) dC1_x= dC_dteta1* x_i(:,i); dC2_x= dC_dteta2* x_i(:,i); dC3_x= dC_dteta3* x_i(:,i); dC4_x= dC_dteta4* x_i(:,i); dC5_x= dC_dteta5* x_i(:,i); dC6_x= dC_dteta6* x_i(:,i); dC7_x= dC_dteta7* x_i(:,i); dC8_x= dC_dteta8* x_i(:,i); dC9_x= dC_dteta9* x_i(:,i); dC_dteta_x=[dC1_x dC2_x dC3_x dC4_x dC5_x dC6_x dC7_x dC8_x dC9_x]; dy_i_dteta_j_2= dC_dteta_x; % TP thu (P.17) % Tinh TP thu ba (P.17) dD1_u=d_D_dteta1*u_i(:,i); dD2_u=d_D_dteta2*u_i(:,i); dD3_u=d_D_dteta3*u_i(:,i); dD4_u=d_D_dteta4*u_i(:,i); dD5_u=d_D_dteta5*u_i(:,i); n dD6_u=d_D_dteta6*u_i(:,i); dD7_u=d_D_dteta7*u_i(:,i); dD8_u=d_D_dteta8*u_i(:,i); dD9_u=d_D_dteta9*u_i(:,i); dD_dteta_u=[dD1_u dD2_u dD3_u dD4_u dD5_u dD6_u dD7_u dD8_u dD9_u]; dy_i_dteta_j_3= dD_dteta_u; % TP thu (P.17) dy_i_dteta_j= dy_i_dteta_j_1 + dy_i_dteta_j_2 + dy_i_dteta_j_3;% Tinh(P.17) % Tinh J, M, g theo cong thuc (3.47, 48) S_i(:,:,i)= dy_i_dteta_j; M_MT(:,:,i) = S_i(:,:,i)'* inv(R)* S_i(:,:,i); g_MT(:,i) = S_i(:,:,i)'* inv(R)*E_i(:,i); J_MT(:,i)= E_i(:,i)'* inv(R)*E_i(:,i); M_T= M_T+ M_MT(:,:,i) ; % ma tran thong tin % gradient teta g_T= g_T+ g_MT(:,i); J_T= J_T+ J_MT(:,i); % tinh ham gia J_TT(:,k)=J_T; % gia tri ham gia qua k lan lap end delta_teta = inv(M_T)*g_T ; % tinh sai so delta_teta Delta_teta(:,k)=delta_teta; % sai so Δθ qua k lan lap teta= teta_old+ delta_teta; % cap nhat gia tri teta E_teta= cov(teta0); % Tinh phuong sai tham so teta E_cov(:,k)=E_teta; % phuong sai θ cac vong lap % gia tri θ qua k vong lap TETA(:,k)=teta; % xac dinh dieu kien dung (3.47) err(k)=(J_T-J_old)/J_old; % tinh cho tham so my_omega if (abs(err(k))