Bộ GIáO DụC V ĐO TạO TRƯờNG ĐạI HọC GIAO THÔNG VậN TảI NGÔ THANH BìNH NÂNG CAO CHấT LƯợNG CHO CáC THIếT Bị ĐịNH Vị DẫN ĐƯờNG Sử DụNG GPS PHụC Vụ BI TOáN GIáM SáT QUảN Lý PHƯƠNG TIệN GIAO THÔNG ĐƯờNG Bộ ngnh: Kỹ thuật điều khiển v tự động hóa MÃ số: 62520216 LUậN áN TIếN SÜ Kü THT Hμ Néi - 2015 Bé GI¸O DơC V ĐO TạO TRƯờNG ĐạI HọC GIAO THÔNG VậN TảI NGÔ THANH BìNH NÂNG CAO CHấT LƯợNG CHO CáC THIếT Bị ĐịNH Vị DẫN ĐƯờNG Sử DụNG GPS PHụC Vụ BI TOáN GIáM SáT QUảN Lý PHƯƠNG TIệN GIAO THÔNG ĐƯờNG Bộ ngnh: Kỹ thuật điều khiển v tự động hãa M· sè: 62520216 LUËN ¸N TIÕN SÜ Kü THUËT Ngời hớng dẫn khoa học: GS.TS Lê Hùng Lân PGS.TS Ngun Thanh H¶i Hμ Néi - 2015 i LỜI CAM ĐOAN Tơi xin cam đoan cơng trình nghiên cứu riêng tôi, tất ý tƣởng tham khảo từ kết nghiên cứu công bố cơng trình khác đƣợc nêu rõ luận án Các số liệu, kết nêu luận án hồn tồn trung thực chƣa đƣợc cơng bố cơng trình khoa học khác Tác giả ii LỜI CẢM ƠN Lời đầu tiên, xin gửi lời cảm ơn chân thành đến hai ngƣời thầy GS.TS Lê Hùng Lân PGS.TS Nguyễn Thanh Hải hƣớng dẫn tận tình thầy q trình tơi học tập, nghiên cứu thực luận án Tôi xin gửi lời cảm ơn giúp đỡ, cộng tác đầy nhiệt tình giáo sƣ đồng nghiệp Nhóm Cơ – Điện tử (Mechatronics Research Group), Khoa Kỹ thuật Công nghiệp (Industrial Engineering), Trƣờng Đại học Trento – Italia, đặc biệt giáo sƣ Francesco Biral Nhân đây, xin gửi lời cảm ơn đến thầy cô đồng nghiệp Khoa Điện - Điện Tử, Trƣờng Đại học Giao thông Vận tải; Vụ Khoa học công nghệ Bộ Giao thông vận tải; Viện Hàn lâm Khoa học Công nghệ Việt Nam; Phịng thí nghiệm – Trung tâm khoa học công nghệ GTVT; Viện Khoa học GTVT; Công ty Than Đèo Nai; Cơng ty Thơng tin Tín hiệu đƣờng sắt Hà nội; Công ty Vận tải Trento – Italia (Trentino Trasporti) đóng góp ý kiến, tạo điều kiện làm việc thuận lợi hợp tác triển khai ứng dụng kết luận án Tôi xin cảm ơn tài trợ kinh phí cho việc nghiên cứu để hoàn thiện nội dung luận án từ Bộ Giáo dục Đào tạo, Trƣờng Đại học Giao thông Vận tải, Tổ chức World Bank, Đại sứ quán Ấn Độ Quỹ học bổng Erasmus Mundus Nhân dịp này, xin gửi lời cảm ơn đến thành viên gia đình ngƣời bạn tơi, ngƣời hết lòng giúp đỡ, động viên, tạo điều kiện thuận lợi suốt thời gian qua để tơi có đƣợc hội hoàn thành tốt luận án iii MỤC LỤC LỜI CAM ĐOAN i LỜI CẢM ƠN ii MỤC LỤC iii DANH MỤC BẢNG vii DANH MỤC HÌNH VẼ vii DANH MỤC CÁC KÝ HIỆU, CHỮ VIẾT TẮT x MỞ ĐẦU xiv Giới thiệu tóm tắt luận án xiv Đặt toán xiv Tính cấp thiết lý chọn đề tài xv Mục đích, đối tƣợng, phạm vi nghiên cứu kết mong đợi luận án xvi Bố cục luận án xvii Ý nghĩa khoa học thực tiễn luận án xix Tính sáng tạo kết nghiên cứu xix Những đóng góp luận án xix TỔNG QUAN VỀ BÀI TOÁN GIÁM SÁT 1 Khái quát toán giám sát sử dụng GPS Phân tích đánh giá cơng trình khoa học liên quan mật thiết đến đề tài nghiên cứu giới Phân tích đánh giá cơng trình khoa học liên quan mật thiết đến đề tài nghiên cứu Việt Nam 4 Đề xuất giải pháp mới, nội dung phƣơng pháp nghiên cứu CHƢƠNG HỆ THỐNG GPS VÀ CÁC HỆ THỐNG HỖ TRỢ iv 1.1 Hệ thống GPS 1.1.1 Hệ trục toạ độ 1.1.2 Cấu trúc hệ định vị sử dụng vệ tinh 1.1.3 Nguyên lý hoạt động 10 1.1.4 Sai số nguyên nhân gây sai số 11 1.2 Hệ thống INS 12 1.2.1 Các hệ tọa độ (Frames) 12 1.2.2 Các thành phần đặc trƣng cho chuyển động 14 1.2.3 Ma trận chuyển vị hệ tọa độ 16 1.2.4 Định vị sử dụng cảm biến quán tính 17 1.2.5 Sai số trình chỉnh ban đầu 18 1.3 Hệ thống định vị tích hợp GPS/INS 20 1.3.1 Bản chất bù INS GPS 20 1.3.2 Các phƣơng pháp tích hợp GPS/INS 21 1.3.3 Bài toán giám sát đối tƣợng áp dụng 23 1.4 Kết luận chƣơng 24 CHƢƠNG LỌC KALMAN VÀ MATLAB TOOL-BOX 25 2.1 Lọc Kalman KF 25 2.1.1 Bản chất toán học 25 2.1.2 Bộ lọc Kalman rời rạc 26 2.2 Bộ lọc Kalman mở rộng EKF 28 2.2.1 Bộ lọc Kalman mở rộng dạng 29 2.2.2 Bộ lọc Kalman mở rộng dạng 30 2.3 Bộ lọc UKF 31 2.4 Đánh giá thuật tốn lọc Kalman kết mơ 34 v 2.5 Kết luận chƣơng 38 CHƢƠNG PHƢƠNG PHÁP TỰ ĐỘNG HIỆU CHỈNH MA TRẬN QUAY CHO HỆ THỐNG INS 39 3.1 Giới thiệu phƣơng pháp tự động hiệu chỉnh ma trận quay 39 3.1.1 Nhận định chung 39 3.1.2 Nguyên tắc sử dụng trận quay R kiểm soát định hƣớng 46 3.1.3 Phát triển phƣơng pháp hiệu chỉnh sở MEMS INS đa trục 48 3.2 Phát triển phƣơng pháp tự động hiệu chỉnh ma trận quay sở MEMS INS 9-DOF 52 3.2.1 Ngun tắc tính tốn 52 3.2.2 Tái chuẩn hóa 55 3.2.3 Hiệu chỉnh trơi dạt góc yaw 60 3.2.4 Hiệu chỉnh trơi góc roll-pitch 64 3.2.5 Phản hồi điều khiển bù sai lệch 66 3.2.6 Kết thử nghiệm với MEMS INS 9-DOF 68 3.3 Kết luận chƣơng 71 CHƢƠNG THIẾT KẾ HỆ THỐNG TÍCH HỢP GPS/INS 72 4.1 Giải pháp sử dụng lọc bù liệu cho thiết bị tích hợp GPS/INS 72 4.1.1 Giải pháp sử dụng lọc Kalman thiết bị xe 72 4.1.2 Phƣơng pháp tính tốn miền rời rạc 75 4.1.3 Giải pháp bù liệu 78 4.2 Đề xuất giải pháp phân tán cho hệ thống tích hợp GPS/INS 81 4.3 Thiết kế hệ thống tích hợp GPS/INS với MEMS INS 9-DOF bus CAN 82 4.4 Ứng dụng phƣơng pháp hiệu chỉnh phần tử ma trận quay phát triển firmware cho hệ thống nhúng xe bus 86 vi 4.5 Thiết kế lọc UKF kết thực tế hệ thống giám sát trạm ứng dụng cho xe bus 93 4.5.1 Đối tƣợng mơ hình xe bus 93 4.5.2 Xây dựng lọc UKF cho xe bus 96 4.5.3 Kết hệ thống giám sát 100 4.6 Kết luận chƣơng 107 KẾT LUẬN VÀ KIẾN NGHỊ 109 I Kết luận luận án 109 II Kiến nghị hƣớng nghiên cứu 109 DANH MỤC CÁC CƠNG TRÌNH CƠNG BỐ CỦA TÁC GIẢ LIÊN QUAN ĐẾN LUẬN ÁN 110 TÀI LIỆU THAM KHẢO 111 A TIẾNG VIỆT 111 B TIẾNG ANH 111 C INTERNET 118 PHỤ LỤC 1: CÁC CƠNG VIỆC HỒN THÀNH TRONG THỜI GIAN THỰC HIỆN LUẬN ÁN II Các cơng trình khoa học cơng bố II Các đề tài, dự án, nhiệm vụ khác chủ trì tham gia V PHỤ LỤC 2: KẾT QUẢ ĐÁNH GIÁ TẠI TRENTO, ITALIA VII PHỤ LỤC 3: MỘT SỐ DỮ LIỆU VÀ ĐOẠN MÃ CHƢƠNG TRÌNH IX Cấu trúc liệu IX Chƣơng trình DCM liệu chế độ tĩnh XIII Script Matlab cho chƣơng trình trạm XXVI vii DANH MỤC BẢNG Bảng 1: Nguồn gốc nguyên nhân gây sai số GPS 12 Bảng 2: Các lỗi hệ strapdown 18 Bảng 3: Các báo cáo khoa học công bố IV Bảng 4: Các đề tài, dự án, nhiệm vụ khoa học VI Bảng 5: Cấu trúc liệu GPS IX Bảng 6: Cấu trúc liệu INS XI Bảng 7: Cấu trúc liệu FMS XII Bảng 8: Dữ liệu INS bị trôi không sử dụng thuật tốn tự động hiệu chỉnh ma trận quay tính từ bƣớc tính thứ (B8.1), 500 (B8.2), 1000 (B8.3), 2000 (B8.4), 3000 (B8.5), 4000 (B8.6) .XX Bảng 9: Dữ liệu INS không bị trôi sử dụng thuật toán thuật toán tự động hiệu chỉnh ma trận quay tính từ bƣớc tính thứ (B9.1), 500 (B8.2), 1000 (B8.3), 2000 (B8.4), 3000 (B8.5), 4000 (B8.6) XXIII DANH MỤC HÌNH VẼ Hình 1: Hệ toạ độ tâm trái đất Oxyz Hình 2: Vệ tinh GPS IIIC quỹ đạo vệ tinh không gian 10 Hình 3: Tín hiệu từ vệ tinh nguyên tắc xác định vị trí đối tƣợng 11 Hình 4: Hệ tọa độ vật thể thành phần đặc trƣng cho chuyển động 12 Hình 5: Hệ toạ độ quán tính ECI 13 Hình 6: Hệ tọa độ CCS hệ NWU 14 Hình 7: Các góc quay roll, pitch, yaw 15 Hình 8: Góc trƣợt α 15 Hình 9: Góc trƣợt β 16 viii Hình 1: Mơ hình hố hoạt động lọc Kalman 25 Hình 2: Cơ chế lọc Kalman 25 Hình 3: Bản chất hoạt động lọc Kalman 26 Hình 4: Biểu diễn hàm trọng lƣợng w (Weighted sigma-points) 31 Hình 5: Tính tốn điểm sigma cho lọc UKF 32 Hình 6: Sơ đồ biến đổi UT 33 Hình 7: Kết sử dụng lọc KF EFK 37 Hình 8: Kết sử dụng lọc UKF 37 Hình 1: Ví dụ mơ tả chuyển động đối tƣợng đặc điểm R 41 Hình 2: Quay hệ tọa độ vật thể P so với hệ tọa độ trái đất G 42 Hình 3: Lệch trực giao tích chéo 45 Hình 4: Cấu trúc lọc chống trôi phần tử ma trận quay William Premerlani Sergiu Baluta 51 Hình 5: Điều chỉnh trƣợt góc hƣớng (yaw correction) 62 Hình 6: Hiệu chỉnh góc quay roll-pitch 65 Hình 7: Cơ chế thuật tốn lọc phần tử ma trận quay 67 Hình 8: Dữ liệu INS bị trơi khơng sử dụng thuật tốn DCM 69 Hình 9: Dữ liệu INS khơng bị trơi có sử dụng thuật tốn DCM 70 Hình 1: Phƣơng pháp truyền thẳng vòng lặp mở 72 Hình 2: Phƣơng pháp phản hồi vịng lặp đóng 73 Hình 3: Sơ đồ tích hợp GPS/INS tập trung vịng mở 73 Hình 4: Sơ đồ tích hợp GPS/INS tập trung vịng đóng 74 XXII (B8.5): Y56.17, P36.35, R147.86 (B8.6): Y-1.26, P34.44, R49.25 Từ bƣớc tính 2000 đến bƣớc tính 3000, góc yall, pitch roll thay đổi giá trị từ (Y64.05, P-9.19, R-146.54) tới (Y56.17, P36.35, R147.86) Từ bƣớc tính 3000 đến bƣớc tính 4000, góc yall, pitch roll thay đổi giá trị từ (Y56.17, P36.35, R147.86) tới (Y-1.26, P34.44, R49.25) Trong chế độ tĩnh mà góc quay sở bị tích lũy sai số, tăng, giảm, làm sai lệch tính tốn dẫn tới sai lệch vận tốc khoảng dịch chuyển Đây lý hệ INS trƣớc không làm việc độc lập đƣợc XXIII Bảng 9: Dữ liệu INS khơng bị trơi sử dụng thuật tốn thuật tốn tự động hiệu chỉnh ma trận quay tính từ bƣớc tính thứ (B9.1), 500 (B8.2), 1000 (B8.3), 2000 (B8.4), 3000 (B8.5), 4000 (B8.6) (B9.1): Y-14.58, P2.22, R1.86 (B9.2): Y-14.36, P2.48, R2.10 Dữ liệu bƣớc tính số nhiễu ngẫu nhiên khởi tạo hệ thống Dữ liệu khơng bị sai số tích lũy, tức khơng thay đổi cộng dồn suốt trình khảo sát mà thay đổi XXIV nhỏ quanh điểm cân Các liệu tự động đƣợc hiệu chỉnh, giá trị liên tục có giá trị xấp xỉ tăng giảm nhỏ dƣới 10 tới max 20 Từ bƣớc tính đầu đến bƣớc tính 500, góc yall, pitch roll thay đổi tăng giảm giá trị từ (Y-14.58, P2.22, R1.86) tới (Y-14.36, P2.48, R2.10) (B9.3): Y-14.19, P2.58, R2.29 (B9.4): Y-14.13, P2.73, R2.51 Từ bƣớc tính 500 đến bƣớc tính 1000, góc yall, pitch roll thay đổi tăng giảm giá trị từ (Y-14.36, P2.48, R2.10) tới (Y-14.19, P2.58, R2.29) XXV Từ bƣớc tính 1000 đến bƣớc tính 2000, góc yall, pitch roll thay đổi tăng giảm giá trị từ (Y-14.19, P2.58, R2.29) tới (Y-14.13, P2.73, R2.51) (B9.5): Y-14.03, P2.77, R2.25 (B9.6): Y-14.04, P2.81, R2.51 Từ bƣớc tính 2000 đến bƣớc tính 3000, góc yall, pitch roll thay đổi tăng giảm giá trị từ (Y-14.13, P2.73, R2.51) tới (Y-14.03, P2.77, R2.25) Từ bƣớc tính 3000 đến bƣớc tính 4000, góc yall, pitch roll thay đổi tăng giảm giá trị từ (Y-14.03, P2.77, R2.25) tới (Y-14.04, P2.81, R2.51) XXVI Script Matlab cho chƣơng trình trạm  Matlab function for busses % -% Dynamical model function for busses function x = busses_f_enhanced(xw,param) dt = param{1}; x = xw(1:9,:); dot_x = zeros(size(x)); dot_x(1,:) = x(5,:); dot_x(2,:) = x(5,:).*cos(x(4,:)); dot_x(3,:) = -x(5,:).*sin(x(4,:)); dot_x(4,:) = x(9,:); dot_x(5,:) = x(8,:)-9.8*sin(x(7,:)); dot_x(6,:) = x(5,:).*sin(x(7,:)); dot_x(7,:) = zeros(1,size(x,2)); dot_x(8,:) = zeros(1,size(x,2)); dot_x(9,:) = zeros(1,size(x,2)); % Euler integration x = x + dt * dot_x; % -% Jacobian of the state transition function in busses function da = busses_df_dx_enhanced(x,param) % Jacobian for ukf dt = param{1}; df = zeros(9,9); df(1,5) = 1; df(2,5) = cos(x(4)); df(2,4) = -x(5).*sin(x(4)); df(3,5) = -sin(x(4)); df(3,4) = -x(5).*cos(x(4)); df(4,9) = 1; df(5,7) = -9.8.*cos(x(7)); df(5,8) = 1; df(6,5) = sin(x(7)); df(6,7) = x(5).*cos(x(7)); da = eye(size(df,1)) + dt * df; % % -% Filtering_autobus_2.m % Script Filtering UKF fprintf ('Entering in the script UKF\n'); fprintf ('\n'); %% generation parameters % Matrix for the states not determined L = [0 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; ; XXVII 0 ]; % Parameters for the dynamic model d_param = {dt2}; h_param = {dt2}; % Initial covariance matrix (take the measures on the states) P0 = diag([0.1 0.25 0.7 0.01 0.001 0.01 0.01 0.001]); % Covariance matrix were not determined (white noise) Qc = diag([0.002 0.1 0.005]); std^2 di alpha_dot, Acc_dot, Omega_dot % Initial values of the states (I assume by the measures) m0=[0; GPSX2(IndexGPSIn); GPSY2(IndexGPSIn); mean(GPSHead2(IndexGPSIn:IndexGPSIn+5)); ; GPSZ2(IndexGPSIn); ; IMUAccLong2(IndexGPSIn); 0]; % calculation of Q Q = L*Qc*L'*dt2; % Handles the functions containing the dynamic and derivatives %(Required view the non-linearity) func_f = @busses_f_enhanced; func_df = @busses_df_dx_enhanced; % Matrix Measures %(sufficient for linear) H = diag([1 1 1 1 1]); % Initialization state and covariance P = P0; m = m0; % Covariance matrix of measures % sFMS XGPS YGPS HeadGPS VGPS ZGPS AlphaGPS ALonIMU OmegaIMU R = diag([100 0.2+1/(3*m(5)+1) 0.6+1.5/(3*m(5)+1) 0.01 0.01 0.1 0.1 0.005]); % Initializing arrays of results MM_EKF = zeros(size(m,1),size(Y2,2)); PP_EKF = zeros(size(m,1),size(m,1),size(Y2,2)); VV_EKF = zeros(size(m,1),size(Y2,2)); % Parameters for the block of the filter at low speed h=0; StopFix2=1; sogliav=1/3.6; %% Unscented Kalman filter fprintf('Running UKF '); Stopi=0; m = m0; P = P0; MM_UKF = zeros(size(m,1),size(Y2,2)); PP_UKF = zeros(size(m,1),size(m,1),size(Y2,2)); XXVIII VV_UKF = zeros(size(m,1),size(Y2,2)); % Filtering with UKF for k=1:size(Y2,2) % Non-augmented UKF [m,P] = ukf_predict1(m,P,func_f,Q,{dt2}); if mod(k*dt2,dt)==0 h=dt2/dt*k; [m,P] = ukf_update1(m,P,Y(:,h),H,R); else [m(8:9),P(8:9,8:9)] = ukf_update1(m(8:9),P(8:9,8:9),Y2(:,k),diag([1 1]),diag([0.1 0.005])); end % Augmented UKF with same sigma points for predict and update steps %[m,P,X_s,w] = ukf_predict3(m,P,func_f,Qc*dt,R,d_param); %[m,P] = ukf_update3(m,P,Y(:,k),H,R,X_s,w,d_param); %H va aumentato! % Block the filter at low speed if StopFix2==1 && k>1 && h>1 if Y(5,h-1)sogliav && Stopi~=0 Stopi=0; end if Stopi>0 m = MM_UKF(:,k-1); P = PP_UKF(:,:,k-1); m(5)=0; end end MM_UKF(:,k) = m; PP_UKF(:,:,k) = P; VV_UKF(:,k) = diag(P); end fprintf('Done!\n'); % -% Fil_FiltraggioPreUKF_2 fprintf('Enter in the script of data processing: \n'); fprintf('\n'); % Imposed sampling frequencies for all data dt=1; % period GPS (1s) dt2=0.2; % Period dt2 = 0.1 StopFix = 0; % The input data are changed by imposing restrictions during a stop DistFix = 0; % Distance is used as the integral of the speed fprintf ('Extracting data '); % I pull the information needed to UKF GPSVel = getData(GPS,'Speed'); GPSTn = getData(GPS,'t'); GPSHead = getData(GPS,'Heading'); XXIX GPSlat = getData(GPS,'LatDeg')+getData(GPS,'LatMin')/60; GPSlon = getData(GPS,'LonDeg')+getData(GPS,'LonMin')/60; GPSAlt = getData(GPS,'Alt'); FMSTime = getData(FMS,'t'); FMSSpeed = getData(FMS,'Vehicle speed'); FMSTotalDistance = getData(FMS,'TotalDist'); IMUTime = getData(IMU,'t'); % IMU varies as a function of the vehicle if (veicolo == 725)||(veicolo == 726) IMUOmega = -getData(IMU,'Gyrz'); %*0.06957 deg/s IMUAccLong = getData(IMU,'Accx'); %*0.004*9.8 m/s2 xGPS=7; yGPS=-1; xIMU=7; yIMU=1; elseif (veicolo == 727) IMUOmega = -getData(IMU,'Gyrz'); IMUAccLong = -getData(IMU,'Accy'); xGPS=7; yGPS=-1; xIMU=7; yIMU=1; elseif (veicolo == 729) IMUOmega = getData(IMU,'Gyrz'); IMUAccLong = -getData(IMU,'Accy'); xGPS=7; yGPS=-1; xIMU=7; yIMU=0; else warning('Omega and Acc may be invalid'); IMUOmega = getData(IMU,'Gyrz'); IMUAccLong = -getData(IMU,'Accy'); xGPS=7; yGPS=-1; xIMU=7; yIMU=0; end % Extract information Ancillary GPSSat = getData(GPS,'SatNum'); GPSTime = getData(GPS,'hours')+getData(GPS,'minutes')/60; FMSPedalPos = getData(FMS,'PedalPos'); if (veicolo == 725)||(veicolo == 726)||(veicolo == 727) FMSTotalFuel = getData(FMS,'TotalFuel'); else FMSFuelRate = getData(FMS,'FuelRate'); end if Radar>0 % Radar tRadar = getData(RAD,'t'); %Check Radar if tRadar(end) change with new function in case you want to impose origin, north, west, n [XYZ, origin, nord, ovest, n] = GPS2XYZ(GPSlon,GPSlat,GPSAlt); GPSX = XYZ(:,1); GPSY = XYZ(:,2); GPSZ = XYZ(:,3); % Precision ground tilt Vsoglia = 8; %10km/h GPSAlpha = []; AlphaTime = []; for i=1:(length(GPSAlt)-1) if GPSVel(i)>Vsoglia GPSAlpha = [GPSAlpha atan((XYZ(i+1,3)-XYZ(i,3))./((XYZ(i+1,1)XYZ(i,1)).^2+(XYZ(i+1,2)-XYZ(i,2)).^2).^0.5)]; AlphaTime = [AlphaTime GPSTn(i)]; end end % Approximation fuel consumption instantaneous fuel consumption VH % fprintf('Done!\n') %% Conditioning and data conversion Dt1 % Interpolated to measure all Dt1 fprintf ('Filtering data1 '); % GPS % The GPS data is lacking in some circumstances, it is therefore necessary % interpolate the data for those moments (default = linear) GPSTn2 = GPSTn(1):dt:GPSTn(length(GPSTn)); GPSVel2 = interp1(GPSTn,GPSVel,GPSTn2); GPSX2 = interp1(GPSTn,GPSX,GPSTn2); GPSY2 = interp1(GPSTn,GPSY,GPSTn2); GPSZ2 = interp1(GPSTn,GPSZ,GPSTn2); GPSHead2 = interp1(GPSTn,GPSHead,GPSTn2)*2*pi/360; GPSSat2 = interp1(GPSTn,GPSSat,GPSTn2,'nearest'); GPSTime2 = interp1(GPSTn,GPSTime,GPSTn2); % Filter and interpolated alpha windowSize = 3; GPSAlpha = filtfilt(ones(1,windowSize)/windowSize,1,GPSAlpha); GPSAlpha2 = interp1(AlphaTime,GPSAlpha,GPSTn2,'linear',0); % I eliminate jumps nell'heading so as to have continuity in the derivative turn=0; XXXI for i=2:length(GPSHead2) if (GPSHead2(i-1)(turn*2*pi+2*pi0.8)) GPSHead2(i:length(GPSHead2))= GPSHead2(i:length(GPSHead2))-2*pi; turn = turn - 1; else if (GPSHead2(i-1)>(turn*2*pi+2*pi0.8))&&(GPSHead2(i)GyrThres)||(IMUOmega(i)GyrThres2)&&(IMUOmega(i)((IMUOmega(i1))+DeltaGyrZ))||((IMUOmega(i))((IMUOmega(i+1))+DeltaGyrZ))||((IMUOmega(i))< ((IMUOmega(i+1))-DeltaGyrZ))) IMUOmega(i)=IMUOmega(i-1); end if (i>Intmean)&&(iAccThres)||(IMUAccLong(i)((IMUAccLong(i1))+DeltaAcc))||((IMUAccLong(i))((IMUAccLong(i+1))+DeltaAcc))||((IMUAccLong( i))2->3) if PreFilter>0 if PreFilter==1||PreFilter==4 % Low Pass Fs = 50; % Sampling Frequency N = 2; % Order Fc = 10; % Cutoff Frequency % Construct an FDESIGN object and call its BUTTER method h = fdesign.lowpass('N,F3dB', N, Fc, Fs); Hd = design(h, 'butter'); %IMUOmega = filter(Hd,IMUOmega); IMUOmega = filtfilt(Hd.sosMatrix,Hd.ScaleValues,IMUOmega); %IMUAccLong = filter(Hd,IMUAccLong); IMUAccLong = filtfilt(Hd.sosMatrix,Hd.ScaleValues,IMUAccLong); elseif PreFilter==2||PreFilter==4 XXXIV %filtro "Dinamico" lpc coeff = lpc(IMUOmega,3); IMUOmega = filter([0 -coeff(2:end)],1,IMUOmega); coeff = lpc(IMUAccLong,3); IMUAccLong = filter([0 -coeff(2:end)],1,IMUAccLong); elseif PreFilter==3||PreFilter==4 % Media mobile windowSize = 5; IMUOmega = filter(ones(1,windowSize)/windowSize,1,IMUOmega); IMUAccLong = filter(ones(1,windowSize)/windowSize,1,IMUAccLong); end end % I sync Omega GPSOmegaEst=diff(GPSHead2)/dt; DT=0; DTdiffmin=0; diffmin=100000000000; while DT=GPSTn2(GPSi)+DT,1); gap2=(IMUOmega(IMUi)*0.06957*2*pi/360-GPSOmegaEst(GPSi))^2; diffO=diffO+gap2; end if diffO