luận án tiến sĩ ứ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 ngồi 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 toá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 TOÁ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 tố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 tố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 tố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 tố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 tố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    β 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     δ Góc lệch cánh lái độ cao đảm bảo máy bay chế độ bay e0 σ 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  A, B,C, D Ma trận hệ thống tuyến tính /s2 m / s2 ax , a y ,az Các thành phần gia tốc hệ tọa độ liên kết  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 , C L ,my Đạo hàm hệ số lực cản, hệ số lực nâng, hệ số mô men gật theo 1/ góc cơng     C D ,C L ,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 /0   C D ,C L ,my Đạo hàm hệ số lực cản, hệ số lực nâng, hệ số mơ men gật theo e e e góc lệch cánh lái độ cao Cx , C y , Cz 1/0   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 ] m / s2 g Gia tốc trọng trường     H Độ cao thiết bị bay [m] I Ma trận mơ men qn tính  kg m2    Ix,Iy, Iz l,l δe L, D m M MA MP Mx,My,Mz mx , my , mz N Oxyz Oxa y a za Ox E y E zE Oxg yg zg P Các mơ men qn tính hệ tọa độ liên kết  kg m2    Sải cánh nâng, cánh lái độ cao [m] Lực nâng lực cản hệ tọa độ tốc độ [N ] Khối lượng thiết bị bay [kg ] Véc tơ mô men tác động lên thiết bị bay [Nm] Véc tơ mơ men khí động tác động lên thiết bị bay [Nm] Véc tơ mô men lực đẩy động tác động lên thiết bị bay [Nm] Các thành phần mơ men khí động hệ tọa độ liên kết [Nm] Các thành phần hệ số mô men khí động hệ tọa độ liên kết Số điểm liệu bay ghi nhận Hệ tọa độ liên kết Hệ tọa độ tốc độ Hệ tọa độ mặt đất Hệ tọa độ mặt đất di động Lực đẩy động [N ] viii N / m3 q Áp suất khí động    Rν Ma trận hiệp phương sai sai số S x Diện tích đặc trưng cánh   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]  m2   Danh mục chữ viết tắt 2.1 Tiếng Anh ANN ACD BP CFD EKF FEM GN IF LIF LS LR 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 : ∂y =C(θ ) ∂θ j 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 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Đ:  d ∂x    dt Để 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) + Γ Phương trình xác định độ nhạy trạng thái: ∂x (i ) ∂θ j =Φ ∂x (i −1) ∂A(θ )    Φ = eA(θ )∆t +Φ ∂A θ ( )[ x (i ) + x(i −1) ] + A(θ )−1 (eA∆t - I)  đó:  Γ  ( ) ∆τ = ∫e A θ dτ B (θ ) = 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); a_x= data(:,5); a_z= data(:,6); alpha = data(:,7); goc_gat = data(:,10); omega_y= data(:,18); delta_e= data(:,17); %#### CAC THAM b_A= 4.6; l= 14.7; P=14500; S= 62.0; m= 24900; Vo=236.11; ro= 0.67; q= ro*Vo^2/2; g=9.806; Ix= 12350; Iy= 62010; Iz= 27600; Ixz= 756; % XAC DINH HE SO LUC NANG VA LUC CAN TU DU LIEU BAY C_z= m*a_z/(q*S); C_x= (m*a_x-P)/(q*S); C_L= -C_z.*cos(alpha)+C_x.*sin(alpha); C_D= -C_x.*cos(alpha)-C_z.*sin(alpha); %### TINH HE SO MÔ MEN GAT TU DU LIEU BAY N=length(goc_gat); giatoc_gat= diff(omega_y); % Tính dao ham 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; 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; i 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) 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 y3= m_y'; my_TS= (inv(X_MT'*X_MT))*X_MT'*y3'; % Nhan 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 data= xls('D:\Thanh_NCS\DLMB_Su-D\bay_bang_1.xls'); docao = data(:,2); tocdo= data(:,3); ro= data(:,20); goc_gat = data(:,10); alpha = data(:,7); a_z= data(:,4); a_x= data(:,5); delta_e= data(:,17); omega_y= data(:,18); delta_TB= data(:,19); %CAC THAM SO DAC TRUNG b_A= 4.645 ; b_A_delta=1.25; % Cung dong cánh lái cao l= 14.7; P=14500; S= 62.0; S_delta= 12.24; % dien tich canh lai cao tham chieu j m= 24900 ; Vo=236.11; q= ro*Vo^2/2; g=9.806; Ix= 12350; Iy= 62010; Iz= 27600; Ixz= 756; %XAC DINH HE SO LUC NANG VA LUC CAN TU DU C_z= m*a_z/(q*S); C_x= (m*a_x-P)/(q*S); C_L= -C_z.*cos(alpha)+C_x.*sin(alpha); C_D= -C_x.*cos(alpha)-C_z.*sin(alpha); %### TINH HE SO MÔ MEN GAT TU DU LIEU BAY #### N=length(goc_gat); giatoc_gat= diff(omega_y); % Tính giatoc_gat1= [giatoc_gat(1:N-1);giatoc_gat(N-2)]; m_y= Iy.*giatoc_gat1/(q*S*b_A); % ### TINH CAC DAN XUAT THEO (2.38) ##### L_alpha=q*S/(m*Vo); L_omega=q*S*b_A/(2*m*Vo*Vo); L_delta=q*S_delta/(m*Vo); M_alpha=q*S*b_A/Iy; M_omega=q*S*b_A*b_A/(2*Vo*Iy); M_delta=q*S_delta*b_A_delta/Iy; %**** TINH CAC DHHSKD THEO PP OEM **** tol= 1e-3; maxstep = 50; teta0=[1 10 0.1 -0.1 -10 -0.1 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= A21= M_alpha* teta_old(4); A22= A=[A11, (1-A12);A21, A22]; % Tinh ma tran B B11= -L_delta*teta_old(3); B21= M_delta*teta_old(6); B= [B11 B12;B21 B22]; % Tinh ma tran C C11=1; C31= -(Vo*L_alpha*teta_old(1))/g; C32= -(Vo*L_omega*teta_old(2))/g; C=[C11 C12; C21 C22; C31 C32]; % Tinh ma tran D D11=0; D12=0; D21=0; D22=0; C12=0; k D31=-(Vo*L_delta*teta_old(3))/g; D32=teta_old(9); D=[D11 D12; D21 D22; 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) d_A_dteta1= [-L_alpha 0; 0 ]; d_A_dteta2= d_A_dteta3= d_A_dteta4= d_A_dteta5= d_A_dteta6= d_A_dteta7= d_A_dteta8= [0 [0 [0 [0 [0 [0 [0 -L_omega; 0 ]; 0; 0]; 0;M_alpha 0]; 0; M_omega]; 0; 0; 0; d_A_dteta9= [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 ]; d_B_dteta4= [0 0; d_B_dteta5= [0 0; d_B_dteta6= [0 0; d_B_dteta7= [0 1;0 0]; d_B_dteta8= [0 0;0 1]; d_B_dteta9= [0 0; % tinh dao ham ma dC_dteta1= [0 0;0 dC_dteta2= [0 0;0 dC_dteta3= [0 0;0 dC_dteta4= [0 0;0 dC_dteta5= [0 0;0 dC_dteta6= [0 0;0 dC_dteta7= [0 0;0 dC_dteta8= [0 0;0 dC_dteta9= [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]; d_D_dteta9= [0 0;0 0;0 1]; % dao ham ma tran D theo teta9 % 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)) A_1=inv(A); % MT nghich dao l I=eye(2); % MT duong cheo GO= (A_1*(PHI-I))*B; % tinh MT GO(P.22) % Tinh u_i va u_TB=(ui1+ui2)/2 u_i1= delta_e'; u_TB1= delta_TB'; u_i2=[ones(1,600)]; u_i=[u_i1;u_i2] ; u_TB=[u_TB1; u_i2]; z_i=[alpha'; omega_y'; x10=sum(alpha)/N; x20=sum(omega_y)/N; x0=[x10;x20]; for i=1:600 x= PHI*x0 + GO*u_TB(:,i); % cong thuc(P.20) y= C*x+ D*u_i(:,i); x_TB_i=(x0+x)/2 ; x_TB(:,i)=x_TB_i; x_i(:,i) y_i(:,i) x0=x; end % Tinh sai lech E_i (3.27)va phuong sai R CT (3.29) E_i=z_i-y_i; % sai so giua dau mo hình va du lieu 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 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); M_T= M_T+ M_MT(:,:,i) ; g_T= g_T+ g_MT(:,i); J_T= J_T+ J_MT(:,i); J_TT(:,k)=J_T; end delta_teta = inv(M_T)*g_T ; Delta_teta(:,k)=delta_teta; teta= teta_old+ delta_teta; E_teta= cov(teta0); E_cov(:,k)=E_teta; 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))