Ứng dụng lý thuyết tối ưu trong điều khiển hệ máy bay không người lái quadrotor

Ứng dụng lý thuyết tối ưu trong điều khiển hệ máy bay không người lái quadrotor Title: Ứng dụng lý thuyết tối ưu trong điều khiển hệ máy bay không người lái quadrotor Authors: Hồ Gia Quyết Advisor: Đào Phương Nam Keywords: Máy bay không người lái; Mô hình Quadroto Issue Date: 2019 Publisher: Trường đại học Bách Khoa Hà Nội Abstract: Tổng quan máy bay không người lái; cấu tạo và mô hình toán học của quadrotor; thiết kế bộ điều khiển tối ưu cho quadrotor; mô phỏng kiểm chứng.

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Hồ Gia Quyết ỨNG DỤNG LÝ THUYẾT TỐI ƢU TRONG ĐIỀU KHIỂN HỆ MÁY BAY KHÔNG NGƢỜI LÁI QUADROTOR LUẬN VĂN THẠC SĨ KỸ THUẬT Chuyên ngành: Kỹ thuật điều khiển tự động hóa Hà Nội, năm 2019 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Hồ Gia Quyết ỨNG DỤNG LÝ THUYẾT TỐI ƢU TRONG ĐIỀU KHIỂN HỆ MÁY BAY KHÔNG NGƢỜI LÁI QUADROTOR LUẬN VĂN THẠC SĨ KỸ THUẬT Chuyên ngành: Kỹ thuật điều khiển tự động hóa NGƢỜI HƢỚNG DẪN KHOA HỌC TS ĐÀO PHƢƠNG NAM Hà Nội, năm 2019 MỤC LỤC TRANG PHỤ BÌA LỜI CAM ĐOAN DANH MỤC TỪ VIẾT TẮT DANH MỤC HÌNH VẼ CHƢƠNG 1: ĐẶT VẤN ĐỀ 1.1 Giới thiệu máy bay không ngƣời lái (Unmanned Aerial Vehicles) 1.2 Tình hình nghiên cứu 1.3 Những vấn đề điều khiển UAV CHƢƠNG 2: CẤU TẠO VÀ MƠ HÌNH TỐN HỌC CỦA QUADROTOR 2.1 Mơ hình Quadrotor 2.1.1 Cấu tạo nguyên lý hoạt động 2.1.2 Động học máy bay 2.1.3 Phƣơng trình Euler – Lagrange CHƢƠNG 3: THIẾT KẾ BỘ ĐIỀU KHIỂN TỐI ƢU CHO QUADROTOR 12 3.1 Thiết kế điều khiển 12 3.2 E-SSPC cho bám quỹ đạo 13 3.3 Điều khiển phi tuyến H  cho hệ chuyển động quay 19 CHƢƠNG : MÔ PHỎNG KIỂM CHỨNG 33 4.1 Sơ đồ Simulink 33 4.2 Kết mô 35 4.2.1 Trƣờng hợp khơng có nhiễu tác động: 35 4.2.2 Trƣờng hợp có nhiễu tác động: 37 KẾT LUẬN 39 PHỤ LỤC: CHƢƠNG TRÌNH MƠ PHỎNG 40 Tài liệu tham khảo: 46 DANH MỤC TỪ VIẾT TẮT UAV : Hệ máy bay không ngƣời lái (Unmanned Aerial Vehicle) : Bộ điều khiển dự báo không gian trạng thái dựa sai E-SSPC lệch mơ hình (Error model- State-Space Predictive Controller) MPC AI : Bộ điều khiển dự báo theo mơ hình (Model Predictive Controller) : Trí thơng minh nhân tạo (Artificial Intelligence) DANH MỤC HÌNH VẼ Hình 1.1: Hình ảnh Quadrotor ………………… Hình 2.1: Cấu tạo hoạt động Quad-rotor .6 Hình 2.2: Momen tương ứng điều khiển góc (a) roll, (b) pitch (c) yaw…… Hình 2.3: Sơ đồ biểu diễn Quadrotor…………………………………………………… Hình 3.1: Cấu trúc điều khiển quadrotor……………………………………………….12 Hình 3.2 Mơ hình hệ phi tuyến H-inf …………………………….………………….…19 Hình 3.3 Cấu trúc điều khiển tối ưu H-inf…………………………………………… 19 Hình 4.1 Cấu trúc điều khiển Simulink ………………………………………….33 Hình 4.2 Vị trí theo trục quadrotor khơng có nhiễu………………….…35 Hình 4.3 Các góc quadrotor khơng có nhiễu………………………………….36 Hình 4.4 Vị trí theo trục quadrotor có nhiễu…………………………… 37 Hình 4.5 Các góc pha quadrotor có nhiễu…………………….………………38 CHƢƠNG 1: ĐẶT VẤN ĐỀ 1.1 Giới thiệu máy bay không ngƣời lái (Unmanned Aerial Vehicles) Hiện nay, số lĩnh vực dân dụng, quân hay khoa học vũ trụ ngƣời dần thay phƣơng tiện bay có ngƣời lái thiết bị bay khơng ngƣời lái, tính ƣu việt nhƣ có khả hoạt động tự động điều khiển từ xa, có khả hoạt động nơi mà ngƣời khó tiếp cận Ngồi cịn đƣợc ứng dụng rộng rãi số lĩnh vực nhƣ quan sát núi lửa, kiểm tra môi trƣờng, gieo trồng, phun thuốc trừ sâu nông nghiệp… Nhờ phát triển công nghệ mà thiết bị điện tử có tốc độ, hiệu suất lƣợng ngày cao kích cỡ, giá thành giảm, hai thập kỷ gần chứng kiến phát triển mạnh mẽ thiết bị máy bay không ngƣời lái (Unmanned Aerial Vehicles - UAV) Điều mở đƣờng cho ứng dụng hữu ích mà UAV phục vụ cho lồi ngƣời, nhƣ nển tảng cho phát triển máy bay cá nhân thực nhỏ gọn Cho đến nay, không tính đến ứng dụng UAV lĩnh vực quân thực nhiệm vụ nguy hiểm tới phi công nhƣ: tiếp cận đến nơi khơng an tồn (qn địch, núi lửa, phá dị mìn), ứng dụng UAV lĩnh vực dân phân loại thành mảng:  Chụp ảnh quay hình: Thu thập thơng tin qua quan sát từ cao, vẽ đồ nhìn đối tƣợng góc nhìn  An ninh: Quan sát khu vực bảo mật nghiêm ngặt, giám sát dịch bệnh, tìm ngƣời tích, …  Mơi trường: Phát cháy rừng, thu thập thông tin mơi trƣờng xác hơn, bảo vệ động vật hoang dã, …  Nghiên cứu khoa học: Điều khiển từ xa thu thập thông tin nghiên cứu đối tƣợng, …  Nông nghiệp: Giám sát nông nghiệp thay cơng việc ngƣời Do tính ứng dụng cao, thiết bị bay không ngƣời lái đƣợc thiết kế đa dạng chuyên dụng cho mục đích khác Có nhiều cách để phân loại UAV nhƣ khả bay xa, kích cỡ hay mục đích sử dụng, nhiên thông thƣờng UAV đƣợc phân loại thành nhóm theo tính đặc trƣng:  Fixed-wing aircraft: Thiết bị bay có cánh cố định, bay với tốc độ cao phù hợp với quãng đƣờng bay xa  Flapping-wing vehicles: Thiết bị bay có cánh đập, giống nhƣ việc mơ lồi chim côn trùng bay  Blimps: Thiết bị bay sử dụng khí nhẹ khơng khí giống nhƣ khinh khí cầu (lighter-than-air UAV)  Rotary wings UAV: Thiết bị bay có cánh quay, đƣợc gọi thiết bị bay cất cánh & hạ cánh theo chiều thẳng đứng (Vertical Take Off & Landing VTOL) Nổi bật ứng dụng nhóm dịng Rotary wings UAV với đặc trƣng Quad-rotor – loại sử dụng rotor cánh quay Đặc điểm dòng cất cánh, hạ cánh theo chiều thẳng đứng, khả giữ ổn định tốt nhƣ kết cấu khí đơn giản Dịng máy bay có khả mang tải nặng kết hợp với khả ổn định tốt, hứa hẹn nhiều ứng dụng hữu ích phục vụ ngƣời 1.2 Tình hình nghiên cứu Trong hai thập kỷ qua, ngoại trừ ngành lƣợng chƣa có nhiều đột phá, phần cứng nhƣ: thiết bị cảm biến vi xử lý có tốc độ, độ xác ngày cao giá thành giảm, công nghệ viễn thông định vị phát triển ứng dụng vào dân nhƣ truyền liệu tốc độ cao & GPS, công nghệ chế tạo phơi với thành cơng tiêu biểu dân hóa máy in 3D, giá mức hợp lý giảm khiến cho việc đƣa UAV vào áp dụng thực tế bƣớc thiết kế điều khiển & lập trình Cũng điều này, lƣợng lớn nghiên cứu điều khiển UAV đƣợc công bố, tập trung chủ yếu vào việc giữ thăng ổn định không chuyển động tới điểm mong muốn Các kỹ thuật điều khiển tiêu biểu kể đến: i Các điều khiển phản hồi đơn giản nhƣ PD, PID điều khiển LQR cho mơ hình đƣợc tuyến tính hóa Đặc điểm điều khiển dễ dàng áp dụng vào thực tế nhƣng chất lƣợng không tốt nhiễu ngoại lực nhiễu bất định ii Các điều khiển nâng cao sử dụng Lý thuyết ổn định Lyapunov Điều khiển trƣợt kết hợp back-stepping nhằm giúp hệ kín ổn định tiệm cận Đặc điểm điều khiển loại đạt tới điểm đặt, có tính bền vững (với điều khiển trƣợt) nhƣng u cầu tính tốn cao thiết kế áp dụng thực tiễn iii Các điều khiển nâng cao sử dụng điều khiển thích nghi bền vững nhằm nâng cao khả chịu nhiễu UAV nhƣ tác động gió, ngoại lực khác, iv Các điều khiển sử dụng phản hồi hình ảnh Camera thu thập hình ảnh gắn gắn ngồi UAV, từ tính tốn tốc độ góc quay đƣa phản hồi cho tín hiệu điều khiển v Các điều khiển khác sử dụng lý thuyết Điều khiển mờ, Mạng nơron hay Trí thông minh nhân tạo (AI) Do chất lƣợng điều khiển ổn định đƣợc đáp ứng tốt, nghiên cứu điều khiển năm trở lại tập trung chủ yếu vào hƣớng:  Thiết kế điều khiển nâng cao thích nghi, bền vững nhằm chống nhiễu môi trƣờng đặc biệt  Tập trung thiết kế điều khiển tối ưu với phiếm hàm mục tiêu nhằm tối ƣu lƣợng, tối ƣu thời gian bay đạt tới điểm đặt trạng thái  Mở rộng mơ hình máy bay nhƣ lắp thêm cánh tay robot điều khiển lên UAV điều khiển ổn định, bám quỹ đạo đặt 1.3 Những vấn đề điều khiển UAV Mặc dù có phát triển rõ ràng việc giữ thăng dòng máy bay không ngƣời lái, nhiên để UAV phát huy đƣợc hết hiệu cần phả cải tiến điều khiển Hình 1.1: Hình ảnh Quadrotor (Nguồn: Internet) Trong đồ án này, đối tƣợng khảo sát đƣợc chọn UAV dòng quad-rotor (quadcopter) tính ổn định, tính ứng dụng cao linh hoạt chuyển động không gian chiều Giống nhƣ mơ hình UAV khác, hệ thống học quadrotor có tính non-holonomic ràng buộc phận chuyển động không trƣợt UAV Vấn đề điều khiển hệ non-honomic chủ đề thu hút mạnh nhà nghiên cứu số lƣợng cơng trình đƣợc cơng bố đồ sộ Tựu chung lại, kết nghiên cứu theo hai hƣớng sau: i Chuyển mơ hình ban đầu hệ đƣợc mơ tả hệ phƣơng trình EulerLagrange sang mơ hình miền không gian trạng thái sử dụng kết lý thuyết điều khiển để giải vấn đề tốn đặt ii Chuyển mơ hình ban đầu hệ đƣợc mơ tả hệ phƣơng trình EulerLagrange sang mơ hình dạng Chained-Form tốn học Sau sử dụng cơng cụ tốn học để giải mục tiêu đề Nếu tính đến đối tƣợng có mơ hình động học chƣa xác định – phải xử lý tốn có bất định tham số & ảnh hƣởng nhiễu cơng trình nghiên cứu giải toán điều khiển bám giá trị đặt cịn gia tăng Mục đích điều khiển dùng cho dòng UAV quadrotor đồ án theo hƣớng khác,đó điều khiển tối ƣu đảm bảo ổn định đạt tới điểm đặt Bên cạnh đó, chúng em nhận thấy UAV loại nhỏ (miniature) có nội lực tác động lên quadrotor nhỏ so với lực tín hiệu điều khiển tác động, mơ hình động học miniature UAV hồn tồn khơng xét tới tính non-holonomic mà điều khiển thiết kế mơ hình đảm bảo chất lƣợng động học Kết nghiên cứu đƣợc thể qua kết mô đáng tin cậy CHƢƠNG : MÔ PHỎNG KIỂM CHỨNG 4.1 Sơ đồ Simulink Cấu trúc điều khiển cho góc quay       vị trí    x y z  hệ máy bay không ngƣời lái Quadrotor nhƣ sau: Hình 4.1 Cấu trúc điều khiển Simulink Có khối cấu trúc điều khiển sơ đồ trên: Khối Trajectory Generator: Có nhiệm vụ tính tốn đƣa giá trị đặt cho vị trí mong muốn (x,y,z) Quarotor, giá trị đƣợc tính theo cơng thức: 33 U1r  m  zr  g  u xr  - xr m y m , u yr  r U1r U1r Đồng thời khối tính tốn ma trận dự báo đầu vào điều khiển , đầu mong muốn (x,y,z) để phục vụ cho khối điều khiển dự báo E-SSPC - Khối E-SSPC: Có nhiệm vụ tính tốn đƣa giá trị điều khiển U1 tối ƣu cho Quadrotor giá trị đặt để đƣa vào khối điều khiển tối ƣu từ giá trị đặt mong muốn vị trí Quadrotor phản hồi trạng thái vị trí góc quay Quarotor - Khối H_inf controller: Có nhiệm vụ tính tốn giá trị moment khiển chuyển động quay Quadrotor từ giá trị đặt để điều khối E- SSPC phản hồi trạng thái góc quay Quadrotor - Khối Quarotor: Là mơ hình động học đối tƣợng 4.2 Thông số điều khiển Các thông số quadrotor: m  0, 74kg l  0, 21m g  9,81m / s I xx  I yy  0, 004kgm , I zz  0, 0084kgm Vị trí ban đầu: , Các tham số  đƣợc chỉnh định nhƣ sau: 1  0,1 2  3  u  1,5 Các thông số điều khiển E-SSPC đƣợc chọn nhƣ sau: 34 3, , Qz = diag([1 0.8 10]); Rz = 0.05; Qxy = diag([25 25 30 25 25 30]); Rxy = diag([30 30]); 4.2 Kết mô 4.2.1 Trường hợp khơng có nhiễu tác động: Xét khơng có nhiễu tác động, quadrotor di chuyển từ vị trí ban đầu đến vị trí Kết thu đƣợc từ mơ nhƣ sau: Đồ thị vị trí x,y,z: Hình 4.2 Vị trí theo trục quadrotor khơng có nhiễu 35 Đồ thị góc pha : Hình 4.3 Các góc quadrotor khơng có nhiễu Nhận xét: - Trong trƣờng hợp khơng có nhiễu tác động lên hệ Quadrotor, với điều khiển dự báo ESSPC vịng ngồi kết hợp điều khiển tối ƣu H  vịng trong, hệ kín bám theo đƣợc giá trị đặt với độ điều chỉnh không đáng kể thời gian độ ngắn, cụ thể: - Đối với biến vị trí x,y,z thời gian độ lần lƣợt 0.3s, 0.2s 0.3s - Đối với biến góc quay thời gian q độ < 0.2s Nhƣ trƣờng hợp khơng tính đến nhiễu tác động, điều khiển xét đảm bảo hệ bám đƣợc giá trị đặt với chất lƣợng tốt 36 4.2.2 Trường hợp có nhiễu tác động: Quadrotor di chuyển từ vị trí ban đầu đến vị trí Nhiễu đƣợc giả thiết nhƣ sau: thời điểm t1= 2s thời điểm t2 = 4s thời điểm t3 = 6s Đồ thị vị trí x,y,z: Hình 4.4 Vị trí theo trục quadrotor có nhiễu 37 Đồ thị góc pha : Hình 4.5 Các góc pha quadrotor có nhiễu Nhận xét: - Trong trƣờng hợp tính đến nhiễu tác động lên hệ Quadrotor, với điều khiển dự báo ESSPC vịng ngồi kết hợp điều khiển tối ƣu H  vòng trong, hệ kín bám theo đƣợc giá trị đặt với thời gian độ ngắn, thời điểm có nhiễu tác động, tín hiệu nhanh chóng quay lại bám giá trị đặt sau khoảng thời gian ngắn (đối với biến vị trí 0.2s biến góc lệch 0.1s), độ điều chỉnh trƣờng hợp không đáng kể Qua nói, điều khiển thiết kế lên hệ Quadrotor xử lý đƣợc ảnh hƣởng nhiễu giúp bám tốt giá trị đặt 38 KẾT LUẬN Bộ điều khiển tối ƣu H-inf kết hợp điều khiển dự báo tích phân đƣợc nói dùng để xử lí hai vấn đề: bám giá trị đặt giảm thiểu tối đa ảnh hƣởng nhiễu Thứ nhất, điều khiển dự báo không gian trạng thái cho chuyển động tịnh tiến đƣợc đặt vịng ngồi (outer-loop) giúp bám tốt giá trị đặt, bên cạnh để khử nhiễu ảnh hƣởng lên hệ, tích phân sai lệch vị trí đƣợc thêm vào Thứ hai, điều khiển tối ƣu H vơ giúp quadrotor ổn định vịng (inner-loop), điều khiển đồng thời khử nhiễu (có đƣa thêm tích phân sai lệch góc vào) Kết mô cho ta thấy đầu bám tốt theo quỹ đạo đặt, đồng thời loại bỏ hồn tồn nhiễu khơng mong muốn từ mơi trƣờng nhƣ sai lệch mơ hình Việc thêm tích phân sai lệch vào tính tốn điều khiển góp phần khơng nhỏ gia tăng xác việc tính tốn tín hiệu điều khiển Tơi mong nhận ý kiến đóng góp quý báu thầy cô Một lần nữa, xin chân thành cảm ơn TS Đào Phương Nam, thầy cô giáo môn Điều khiển tự động, viện Điện, trường Đại học Bách Khoa Hà Nội giúp đỡ tơi hồn thành luận văn 39 PHỤ LỤC: CHƢƠNG TRÌNH MƠ PHỎNG Phƣơng trình động học Quadrotor function [ddx,ddy,ddz,ddphi,ddtheta,ddpsi] = fcn(phi,theta,psi,dphi,dtheta,dpsi,U1,Ax,Ay,Az,tau_phi,tau_theta,tau_psi) % Parameters m = 0.74; %l = 0.21; g = 9.81; Ixx = 0.004; Iyy = 0.004; Izz = 0.0084; % Translation Dynamics ddx = ((cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi))*U1+Ax)/m; ddy = ((sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi))*U1+Ay)/m; ddz = -g + (cos(theta)*cos(phi)*U1 + Az)/m; % Rotation Dynamics M = [Ixx Ixx*sin(theta); Iyy*cos(phi)^2+Izz*sin(phi)^2 Izz)*cos(phi)*sin(phi)*cos(theta); -Ixx*sin(theta) (Iyy-Izz)*cos(phi)*sin(phi)*cos(theta) Ixx*sin(theta)^2 + Iyy*sin(phi)^2*cos(theta)^2 + Izz*cos(phi)^2*cos(theta)^2]; (Iyy- c11 = 0; c12 = (Iyy-Izz)*(dtheta*cos(phi)*sin(phi) + dpsi*sin(phi)^2*cos(theta)) + (Izz-Iyy)*dpsi*cos(phi)^2*cos(theta) - Ixx*dpsi*cos(theta); c13 = (Izz-Iyy)*dpsi*cos(phi)*sin(phi)*cos(theta)^2; c21 = (Izz-Iyy)*(dtheta*cos(phi)*sin(phi) + dpsi*sin(phi)^2*cos(theta)) + (Iyy-Izz)*dpsi*cos(phi)^2*cos(theta) + Ixx*dpsi*cos(theta); c22 = (Izz-Iyy)*dphi*cos(phi)*sin(phi); c23 = -Ixx*dpsi*sin(theta)*cos(theta) + Iyy*dpsi*sin(phi)^2*cos(theta)*sin(theta) + Izz*dpsi*cos(phi)^2*sin(theta)*cos(theta); c31 = (Iyy-Izz)*dpsi*cos(theta)^2*sin(phi)*cos(phi) Ixx*dtheta*cos(theta); c32 = (Izz-Iyy)*(dtheta*cos(phi)*sin(phi)*sin(theta) + dphi*sin(phi)^2*cos(theta)) + (Iyy-Izz)*dphi*cos(phi)^2*cos(theta) + Ixx*dpsi*sin(theta)*cos(theta) Iyy*dpsi*sin(phi)^2*sin(theta)*cos(theta) Izz*dpsi*cos(phi)^2*sin(theta)*cos(theta); c33 = (Iyy-Izz)*dphi*cos(phi)*sin(phi)*cos(theta)^2 Iyy*dtheta*sin(phi)^2*cos(theta)*sin(theta) Izz*dtheta*cos(phi)^2*cos(theta)*sin(theta) + Ixx*dtheta*cos(theta)*sin(theta); C = [c11 c12 c13; c21 c22 c23; c31 c32 c33]; %epsilon = 0.001*eye(3) ; %M = M + epsilon; tau = [tau_phi,tau_theta,tau_psi]'; % eta = [phi theta psi]'; 40 d_eta = [dphi,dtheta,dpsi]'; temp = inv(M); dd_eta = temp*(tau - C*d_eta); ddphi = dd_eta(1); ddtheta = dd_eta(2); ddpsi = dd_eta(3); Thuật toán cho điều khiển dự báo E-SSPC function [U1,eta_r] = fcn(Delta_x,Delta_y,Delta_z,Delta_ix,Delta_iy,Delta_iz,Delta_dxr,Delta_dy r,Delta_dzr,uxr,uyr,U1r,Delta_xr,Delta_yr,Delta_zr,Delta_uxr,Delta_uyr,De lta_U1r,phi,theta,psi) %#codegen m = 0.74; g = 9.81; delta_t = 0.01; %Define input error ex = [Delta_x;Delta_dxr;Delta_ix]; ey = [Delta_y;Delta_dyr;Delta_iy]; ez = [Delta_z;Delta_dzr;Delta_iz]; Az = [1 delta_t 0; 0; delta_t 1]; Bz = [0; delta_t*cos(phi)*cos(theta)/m; 0]; Pz = [Az;Az^2;Az^3]; Hz = [Bz [0;0;0] [0;0;0];Az*Bz Bz [0;0;0];Az^2*Bz Az*Bz Bz]; % Qz = [1 0; 0.8 0; 0 10]; Qz = diag([1,0.8,10]); Qz_bar = kron(eye(3),Qz); Rz_bar = 0.05*[1 0;0 0;0 1]; % Tinh delta_u(k) delta_uz = (Hz'*Qz_bar*Hz+Rz_bar)\(Hz'*Qz_bar*(Delta_zrPz*ez)+Rz_bar*Delta_U1r); % Tinh U1 U1 = delta_uz(1) + U1r; %Tinh ux,uy Ax = [1 delta_t 0; 0; delta_t 1]; Ay = Ax; Bx = [0;delta_t*U1/m; 0]; By = Bx; Hx = [ Bx [0;0;0] [0;0;0] [0;0;0] [0;0;0]; Ax*Bx Bx [0;0;0] [0;0;0] [0;0;0]; Ax^2*Bx Ax*Bx Bx [0;0;0] [0;0;0]; Ax^3*Bx Ax^2*Bx Ax*Bx Bx [0;0;0]; Ax^4*Bx Ax^3*Bx Ax^2*Bx Ax*Bx Bx]; Hy = Hx; Px = [ Ax; Ax^2; Ax^3; Ax^4; Ax^5]; Py = Px; Qx = kron(eye(5),diag([25 25 30])); Qy = Qx; Rx = kron(eye(5),30); Ry = Rx; 41 % Tinh ux,uy delta_ux = (Hx'*Qx*Hx+Rx)\(Hx'*Qx*(Delta_xr - Px*ex) + Rx*Delta_uxr); delta_uy = (Hy'*Qy*Hy+Ry)\(Hy'*Qy*(Delta_yr - Py*ey) + Ry*Delta_uyr); ux = uxr + delta_ux(1); uy = uyr + delta_uy(1); % Tinh phi_r, theta_r sin_phir = ux*sin(psi)-uy*cos(psi); if ((sin_phir>=-1)&&(sin_phir=-1)&&(sin_thetar