Bộ lọc kalmanfilter xử lý tín hiệu số

HỌC VIỆN CÔNG NGHỆ BƯU CHÍNH VIỄN THÔNG - - BÁO CÁO TIỂU LUẬN MÔN: XỬ LÝ TÍN HIỆU SỐ NÂNG CAO Đề tài: Tìm hiểu lọc Kalman Giảng viên hướng dẫn: Nhóm SV thực hiện: Ts NGUYỄN NGỌC MINH TRẦN VĂN HIỆU QUÁCH VĂN PHI VANHNARLAK SOULIGNAVONG Lớp : M14CQTE02-B Hà nội, năm 2015 Tìm hiểu lọc Kalman MỤC LỤC LỚP: M14CQTE02-B Page i Tìm hiểu lọc Kalman DANH MỤC HÌNH VẼ LỚP: M14CQTE02-B Page ii Tìm hiểu lọc Kalman LỜI MỞ ĐẦU Trong năm gần đây, cảm biến thiết bị đo lường xuất nhiều thiết bị tự động, dân dụng thiết bị công nghiệp Trong cảm biến thường phận thiết yếu mạch đo lường dùng để thu nhập thông số môi trường(như cảm biến nhiệt độ, độ ẩm, ấp suất,…), thông số đo đạc trạng thái học(như cảm biến la bàn số, cảm biến vận tốc, gia tốc,…), Nhưng bất cập cảm biến giá trị đọc từ cảm biến thường chứa nhiễu tác động môi trường, cảm biến, trình chuyển đổi Do giá trị đọc thường sai số lớn Để giải vấn đề nhiễu thường sử dụng lọc thích hợp với sai số ảnh hưởng đến cảm biến Một số phương pháp lọc nhiễu thường sử dụng là: lọc trung bình, lọc thông thấp, thông cao lọc Kalman Với ưu điểm vượt trội, tiềm thuật toán Kalman vào thực tế việc áp dụng để lọc nhiễu tín hiệu khả quan, điều đặt cho chúng em mong muốn nghiên cứu để nắm rõ tiến tới làm chủ phương pháp Định hướng chúng em tìm hiểu lý thuyết lọc Kalman, tìm hiểu mô hình toán học phạm vi ứng dụng Sau áp dụng mô hình toán học lọc hệ thống đo lường cụ thể lọc nhiễu cho cảm biến gia tốc Trong phạm vi tiểu luận này, chúng em xin trình bày lý thuyết lọc Kalman, nội dung bao gồm mục sau: - Lý thuyết ước lượng Mô hình toán học lọc Kalman Các ứng dụng lọc Kalman Mô lọc Kalman cho hệ thống tuyến tính rời rạc Matlab Lớp: M14CQTE02-B Page Tìm hiểu lọc Kalman I LÝ THUYẾT BỘ LỌC KALMAN Vào năm 1960, R.E Kalman công bố báo tiếng giải pháp truy hồi để giải toán lọc thông tin rời rạc truyến tính (discrete data linear filtering) Tên đầy đủ báo “A New Approach to Linear Filtering and Prediction Problems” Từ đến với phát triển tính toán kỹ thuật số, lọc Kalman trở thành chủ đề nghiên cứu sôi ứng dụng nhiều ngành kỹ thuật công nghệ khác nhau: tự động hóa, định vị viễn thông nhiều lĩnh vực khác Một cách khái quát, lọc Kalman tập hợp phương trình toán học mô tả phương pháp tính toán truy hồi hiệu qủa cho phép ước đoán trạng thái trình cho trung bình phương sai độ nhỏ Bộ lọc Kalman hiệu việc ước đoán trạng thái khứ, tương lai chí tính xác hệ thống mô không khẳng định Lý thuyết ước lượng 1.1 Khái niệm Trong thống kê, ước lượng giá trị tính toán từ mẫu thử người ta hy vọng giá trị tiêu biểu cho giá trị cần xác định tập hợp Người ta tìm ước lượng cho ước lượng “không chệch”, hội tụ, hiệu vững(robust) 1.2 Đánh giá chất lượng Một ước lượng giá trị x tính toán mẫu lấy cáchngẫu nhiên, giá trị x biến ngẫu nhiên với kì vọng E(x) phương sai V(x) Nghĩa giá trị x dao động tùy theo mẫu thử, có hội để xác giá trị X mà ước lượng Mục đích ta muốn kiểmsoát sai lệch giá trị x giá trị X Một biến ngẫu nhiên dao động xung quanh giá trị kì vọng Ta muốn kì vọng x phải X Khi ta nói ước lượng không chệch Trung bình tích lũytrong ví dụ chiều cao trung bình trẻ 10 tuổi ước lượng đúng, ướclượng tổng số cá hồ tính ví dụ ước lượng không đúng, đólà ước lượng thừa: trung bình tổng số cá ước lượng lớn tổng số cá có thực hồ Ta muốn mẫu thử rộng, sai lệch x X nhỏ Khi Lớp: M14CQTE02-B Page Tìm hiểu lọc Kalman ta nói ước lượng hội tụ Định nghĩa theo ngôn ngữ toán học sau: lim p(| xn − X |> ε ) = (Xn) n→∞ hội tụ với số thực dương (xác suất để sai lệch với giá trị thực cần ước lượng lớn tiến kích cỡ mẫu thử lớn) Biến ngẫu nhiên dao động quanh giá trị kì vọng Nếu phương sai V(x) bé, dao động yếu Vì ta muốn phương sai ước lượng nhỏ Khi ta nói ước lượng hiệu Cuối cùng, trình điều tra, xuất giá trị “bất thường” (ví dụ có trẻ 10 tuổi cao 1,80 m) Ta muốn giá trị bất thường không ảnh hưởng nhiều đến giá trị ước lượng Khi ta nói ước lượng vững Có thể thấy trung bình tích lũy ví dụ chiều cao trung bình trẻ 10 tuổi ước lượng vững 1.3 Kỳ vọng (Expectation) Định nghĩa: Giả sử � đại lượng ngẫu nhiên rời rạc nhận giá trị �1, �2, … , �� với xác suất tương ứng �1, �2, … , �� Khi kỳ vọng X, ký hiệu (�) hay � xác định công thức: n E ( x) = ∑ xi pi i =1 (1.1) Nếu � đại lượng ngẫu nhiên liên tục có hàm mật độ xác suất (�) kỳ vọngcủa � là: +∞ E ( x ) = ∫ xf ( x )dx −∞ (1.2) Tính chất i �(�) = � ii �(� �) = � (�), với � số iii.�(� + �) = �(�) + �(�) iv Nếu X Y hai đại lượng ngẫu nhiên độc lập thì: v �(� �) = �(�) �(�) Ý nghĩa: Kỳ vọng đại lượng ngẫu nhiên giá trị trung bình (theo xác suất) đại lượng ngẫu nhiên Nó điểm trung tâm phân phối mà giá trị cụ thể X tập trung quanh Lớp: M14CQTE02-B Page Tìm hiểu lọc Kalman 1.4 Phương sai (Variance) Định nghĩa: Phương sai (trung bình bình phương độ lệch) đại lượng ngẫunhiên X, ký hiệu (�) hay (�) xác định công thức: Var( X ) = E[( X − µ ) ] (1.3) Nếu X đại lượng ngẫu nhiên rời rạc nhận giá trị �1, �2, … , �� với xác suất tương ứng �1, �2, … , �� thì: n Var( X ) = ∑ [ xi − µ ]2 pi i =1 (1.4) Nếu X đại lượng ngẫu nhiên liên tục có hàm mật độ xác suất (�) thì: +∞ Var ( x) = ∫ [ x − µ ]2 f ( x)dx −∞ (1.5) Trong thực tế ta thường tính phương sai công thức: Var ( x ) = E[ X ] − E[ X ]2 (1.6) Tính chất: i.Var (�) = ii Var(� �) = � ���(�); iii Nếu X, Y biến ngẫu nhiên độc lập thì: �ar(� + �) = ���(� − �) = ���(�) + ���(�) (1.7) Ý nghĩa: � − � độ lệch khỏi giá trị trung bình Do phương sai (�) gọi làtrung bình bình phương độ lệch Nên phương sai phản ánh mức độ phân tán giá trị đại lượng ngẫu nhiên quanh giá trị trung bình hay kỳ vọng Đại lượng ngẫu nhiên có phương sai lớn giá trị phân tán ngược lại 1.5 Độ lệch chuẩn Định nghĩa: Độ lệch chuẩn đại lượng ngẫu nhiên X, ký hiệu (�) xác định công thức: σ ( x) = Var ( x) (1.8) Lớp: M14CQTE02-B Page Tìm hiểu lọc Kalman 1.6 Hiệp phương sai (Covariance) Cho biến ngẫu nhiên X Y, ta có định nghĩa hiệp phương sai X Y, kýhiêu (�, �): Cov( X , Y ) = E[( X − µ x )(Y − µ y )] (1.9) Trong , �� kỳ vọng X, Y Một công thức tương đương hiệp phương sai: Cov( X , Y ) = E[ XY ] − µ x µ y (1.10) Ý nghĩa hiệp phương sai biến thiên biến ngẫu nhiên: Nếu biến có xu hướng thay đổi (nghĩa là, biến có giá trị cao kỳ vọng biến có xu hướng cao kỳ vọng), hiệp phương sai hai biến có giá trị dương Mặt khác, biến nằm giá trị kỳ vọng biến có xu hướng nằm giá trị kỳ vọng, hiệp phương sai hai biến có giá trị âm Nếu biến ngẫu nhiên độc lập (�, �) = nhiên điều ngược lại không Các biến ngẫu nhiên mà có hiêp phương sai gọi không tươngquan (uncorrelated), chúng độc lập không Như X, Y độc lập ta có E[��] = Tính chất - Cov( X , Y ) = Var( X ) Cov( X , Y ) = Cov(Y , X ) Cov(aX , bY ) = abCov( X , Y ) Cov( X + X , Y1 + Y2 ) = Cov( X , Y1 ) + Cov ( X , Y2 ) + Cov ( X , Y1 ) + Cov ( X , Y2 ) Var ( X + Y ) = Var ( X ) + Var (Y ) + 2Cov ( X , Y ) 1.7 Ma trận hiệp phương sai Như vừa trình bày, hiệp phương sai đại lượng tính toán tương quan biến ngẫu nhiên Lớp: M14CQTE02-B Page Tìm hiểu lọc Kalman Vậy giả sử có vector biến ngẫu nhiên có phần tử �1, �2, �3 Nếu ta muốn tính toán tương quan tất cặp biến ngẫu nhiên ta phải tính tất hiệp phương sai ���(�1, �2), ���(�1, �3), ���(�2, �3) Một cách tổng quát, ma trận hiệp phương sai đời phép ta tính tất ��� biến ngẫu nhiên vector biến ngẫu nhiên Cho vector biến ngẫu nhiên X chứa n biến ngẫu nhiên, ma trận hiệp phương sai X, kỹ hiệu ∑, định nghĩa là:  Cov( X , Y1 ) Cov( X , Y2 ) Cov( X , Y ) Cov( X , Y ) 2 ∑=  M  Cov( X n , Y1 ) Cov( X n , Y2 ) Với Cov( X , Yn )  Cov( X , Yn )    Cov( X n , Yn )   X1  X =    X n  Quan sát đường chéo ma trận hiệp phương sai (i=j) ta thấy phương sai, ���(�� , �� ) = ���(�� ) 1.8 Phân phối chuẩn (phân phối Gaussian) Trong thực tế, người ta thường sử dụng phân phối xác suất có tên phân phối chuẩn (normaldistribution) hay phân phối Gaussian.Một biến ngẫu nhiên X gọi có phân phối Gaussian có hàm mật độ hàm Gaussian, ký hiệu �~(�, �) gọi X có phân phối chuẩn với tham số �, �.Khi hàm mật độ X là: f ( x, σ , µ ) = σ 2π e − ( x − µ )2 2σ Với phân phối xác suất trên, người ta tính �, � kỳ vọng độ lệch chuẩn X Dưới đồ thị số phân phối chuẩn Lớp: M14CQTE02-B Page Tìm hiểu lọc Kalman Hình 1.1: Đồ thị số phân phối chuẩn Quan sát đồ thị ta thấy phân phối chuẩn có dạng chuông Giá trị kỳ vọng X �=� trục đối xứng Độ lệch chuẩn � (hay phương sai σ2 ) lớn đồ thị bẹt, nghĩa giá trị phân tán xa kỳ vọng Trong thực tế, loại nhiễu hệ thống đo lường mô cách xác nhiễu trắng cộng Hay nói cách khác tạp âm trắng Gaussian loại nhiễu phổ biến hệ thống đo lường Loại nhiễu có mật độ phổ công suất đồng miền tần số biên độ tuân theo phân bố Gaussian Theo phương thức tác động nhiễu Gaussian nhiễu cộng Vậy hệ thống đo lường phổ biến chịu tác động nhiễu Gaussian trắng cộng (AWGN) Lớp: M14CQTE02-B Page 10 Tìm hiểu lọc Kalman Trong K k' +1 K k +1 ma trận hiệu chỉnh bù hay ma trận độ lời (của K k' +1 K k +1 kích thước khác nhau) Vấn đề tìm để tối thiểu hóa điều kiện ước lượng trung bình bình phương sai số Sai số dự đoán cho ˆk +1|k +1 − xk +1 x% k +1|k +1 = x (2.12) 2.2.4 Điều kiện không chệch E  xˆk +1|k +1  − E  xk +1  xˆk |k Để lọc không chệch yêu cầu Giả sử ước lượng không chệch Kết hợp phương trình (2.11) (2.12) tính kỳ vọng E  xˆk +1|k +1  = E  K k' +1 xˆk +1|k + K k +1 H k +1 xk +1 + K k +1vk +1  = K k' +1 E  xˆk +1|k  + K k +1 H k +1 E [ xk +1 ] + K k +1 E [ vk +1 ] Vì E [ vk +1 ] (2.13) = 0, dự đoán không chệch: E  xˆk +1|k +1  = E  Fk xˆk |k + Gk uk  = Fk E  xˆk|k  + Gk uk = E [ xk +1 ] Do kết hợp phương trình (2.13) (2.14) E  xˆk +1|k +1  = ( K k' +1 + K k +1 H k +1 ) E [ xk +1 ] Và điều kiện để xˆk +1|k +1 không chệch yêu cầu K k' +1 + K k +1 H k +1 = I Lớp: M14CQTE02-B Page 18 (2.14) Tìm hiểu lọc Kalman Hay K k' +1 = I − K k +1H k +1 (2.15) Để ước lượng không chệch yêu cầu xˆk +1|k +1 = ( I − K k +1 H k +1 ) xˆk +1|k + K k +1 zk +1 = xˆk +1|k + K k +1[zk +1 − H k +1 xˆk +1|k ] (2.16) Trong K gọi độ lời lọc Kalman (Kalman gain) zˆk +1|k H k +1 xˆk +1|k hiểu quan sát hay đo lường dự đoán zˆ k +1|k = E[ zk +1 | Z k ] = E[ H k +1 xk +1 + vk +1 | Z k ] =H k +1 xˆk +1|k (2.17) Đặt rk +1 độ lệch đo lường thể sai khác giá trị đo lường ước lượng zˆk +1|k zk +1 , biểu diễn rk +1 = zk +1 − H k +1 xˆk +1|k E  rk +1 | Z k  = E  zk +1 − zˆk +1|k | Z k  = E  zk +1 | Z k  − zˆk +1|k =0 (2.19) Kết hợp (2.16) (2.18) ta xˆk +1|k +1 = xˆk +1|k + K k +1rk +1 (2.20) Lớp: M14CQTE02-B Page 19 Tìm hiểu lọc Kalman Hiệp phương sai độ lệch đo lường Sk +1 cho bởi, S k +1 = E[ rk +1rkT+1 ] = E ( zk +1 − H k +1 xˆk +1|k )( zk +1 − H k +1 xˆk +1|k )T  S k +1 = H k +1 Pk +1|k H kT+1 + Rk +1 (2.21) Chúng ta xác định hiệp phương sai sai số dự đoán phương trình (2.10) Bây tính toán hiệp phương sai sai số điều chỉnh T k %  Pk +1|k +1 = E  ( x% k +1|k +1 )( xk +1|k +1 ) | Z  = E ( xk +1 − xˆk +1|k +1 )( xk +1 − xˆk +1|k +1 )T  = cov( xk +1 − xˆk +1|k +1 ) = cov(( I − K k +1 H K +1 )( xk +1 − xˆk +1|k )) + cov( K k +1vk +1 ) = ( I − K k +1 H K +1 ) cov( xk +1 − xˆk +1|k )( I − K k +1 H K +1 )T + K k +1 cov(vk +1 ) K kT+1 Pk +1|k +1 = ( I − K k +1 H K +1 ) Pk +1|k ( I − K k +1 H K +1 )T + K k +1 Rk +1K kT+1 (2.22) Ta tính toán Pk +1|k +1 theo cách khác sau Pk +1|k +1 = cov( xk +1 − xˆk +1|k +1 ) = cov( xk +1 − xˆk +1|k − K k +1rk +1 ) = cov( xk +1 − xˆk +1|k ) − cov( K k +1rk +1 ) = Pk +1|k − K k +1S k +1 K kT+1 (2.23) Trong Lớp: M14CQTE02-B Page 20 Tìm hiểu lọc Kalman Pk +1|k = cov( xk +1 − xˆk +1|k ) Rk +1 = cov ( vk +1 ) = E ( vk +1vkT+1 ) Vì hiệp phương sai ước lượng điều chỉnh biểu diễn qua hiệp phương sai dự đoán Pk +1|k , nhiễu đo lường Rk +1 ma trận độ lời Kalman K k +1 2.2.6 Độ lời Kalman Mục tiêu để tối thiểu hóa trung bình bình phương sai số ước lượng có điều kiện với độ lời Kalman K ( x%k +1|k+1) ( x% ) | Z   L = E  K k +1  T k k +1|k +1 ( x% ) ( x% ) | Z  ÷   = trace  E  K k +1   = trace K k +1 (P T k k +1|k +1 k +1|k +1 k +1|k +1 ) (2.24) Với ma trận A ma trận đối xứng B ta có ( ∂ trace( A ∂A B A ) ) = AB T Kết hợp (2.23) (2.24) lấy vi phân ma trận độ lời đặt kết ta ∂L = − ( − I K k +1H k +1) P k +1|k H k +1 + K k +1R k +1 = ∂K k +1 T Sắp xếp lại đưa phương trình cho ma trận độ lời K k +1 = P k +1|k H k +1 H k +1P k +1|k H Tk +1+ R k +1 T Kết hợp với (2.21) ta Lớp: M14CQTE02-B Page 21 −1 (2.25) Tìm hiểu lọc Kalman K T k +1 −1 = P k +1|k H k +1S k +1 (2.26) Cùng với phương trình 2.16, định nghĩa ước lượng tối ưu tuyến tính trung bình bình phương sai số Từ (2.26) ta có T T T K k +1S k +1K k +1 = P k +1|k H k +1K k +1 (2.27) Kết hợp (2.23) (2.27) P k +1|k +1 = P k +1|k ( I − H k +1K k +1) T = = (P (I k +1|k (I T − H Tk +1K Tk +1 − H Tk +1K Tk +1 )) T ) ( P k +1|k ) T T = ( I − K k +1H k +1) P k +1|k (2.28) 2.2.7 Tóm tắt phương trình lọc Kalman Trong phần tóm tắt phương trình tổng quát giải thuật lọc Kalman Giải thuật bao gồm trình: trình ước lượng trình điều chỉnh • Quá trình dự đoán xˆk|k Bộ lọc Kalman dựa vào trạng thái ước lượng điều chỉnh xk để ước lượng trạng thái xˆk +1|k – ước lượng dự đoán Trạng thái dự đoán: Lớp: M14CQTE02-B Page 22 xk +1 - ước lượng cho phép đo zk +1 Tìm hiểu lọc Kalman xˆ k +1|k = F k xˆ k|k + G ku k (2.29) Hiệp phương sai ước lượng dự đoán: P k +1|k = F k P k |k F k + Q k T (2.30) Đo lường dự đoán: zˆ k +1|k = H k +1xˆ k +1|k (2.31) • Quá trình điều chỉnh Độ lệch đo lường: r k +1 = z k +1− H k +1 xˆ k +1|k (2.32) Hiệp phương sai độ lệch: T S k +1 = H k +1P k +1|k H k +1 + R k +1 (2.33) Độ lời Kalman: T −1 K k +1 = P k +1|k H k +1S k +1 (2.34) Trạng thái ước lượng điều chỉnh: xˆ k +1|k +1 =x ˆ k +1|k + K k +1r k +1 (2.35) Hiệp phương sai ước lượng điều chỉnh: P k +1|k +1 = ( I − K k +1H k +1) P k +1|k Lớp: M14CQTE02-B Page 23 (2.36) Tìm hiểu lọc Kalman Hình 2.2: Tóm tắt trình khởi tạo Kalman Cùng với điều kiện ban đầu ước lượng ma trận hiệp phương sai lỗi (phương trình 2.6) định nhĩa giải thuật rời rạc hóa thời gian đệ quy để xác định hiệp phương sai ước lượng tuyến tính tối thiểu gọi lọc Kalman II ỨNG DỤNG CỦA BỘ LỌC KALMAN Bởi lọc Kalman giải số vấn đề lọc nhiễu tối ưu cho ước lượng nên ứng dụng rộng rãi Ngày Kalman ứng dụng nhiều ô tô tự lái có khả thay người vận hành xe, chương trình máy tính cài sẵn lọc Kalman có nhiệm vụ điều khiển xe Những xe chí giới thiệu an toàn xe lái người số trường hợp Một ứng dụng khác, không thích thú lắm, tên lửa không đối không (air-to-air missile: AAM) Đó tên lửa dẫn hướng việc bắn từ máy bay để tiêu diệt máy bay khác Tên lửa dẫn hướng hoạt động theo nguyên lý Lớp: M14CQTE02-B Page 24 Tìm hiểu lọc Kalman phát mục tiêu (thông thường rada hồng ngoại, sử dụng Lazer quang học) sau tự động dẫn đến mục tiêu nhờ trình ước lượng Kalman Ngoài lọc Kalman áp dụng nhiều vào hệ thống theo dõi mục tiêu di động mạng cảm biến không dây Do nhiễu đo lường cảm biến nên kết thu thường không xác, có sai số lớn so với thực tế Bộ lọc Kalman áp dụng để lọc nhiễu, dự đoán, ước lượng trạng thái mục tiêu vị trí, tốc độ quỹ đạo Nhờ có trình dự đoán điều chỉnh lọc Kalman góp phần quan trọng vào việc quản lý trạng thái cảm biến làm giảm thiểu lượng tiêu thụ tăng chất lượng theo dõi kéo dài thời gian sống mạng Một số ứng dụng liệt kê từ viết Kalman Filter Wikipedia [5]: − − − − − − − − − − − Lái tự động máy bay (Autopilot) Ước lượng trạng thái sạc pin (Battery state of charge (SoC) estimation) Giao diện tương tác với máy tính não (Brain–computer interface) Định vị chuyển động (Dynamic positioning) Các ứng dụng kinh tế, đặc biệt kinh tế vĩ mô, time series, econometrics Hệ thống dẫn đường quán tính (Inertial guidance system) Theo dõi radar (Radar tracker) Hệ thống định vị vệ tinh (Satellite navigation systems) Dự báo thời tiết (Weather forecasting) Hệ thống định vị (Navigation Systems) Mô hình hóa chiều (3D-Modelling) Ở Việt Nam có số ứng dụng như: − Ứng dụng lọc Kalman phân tích biến dạng nhà cao tầng xạ nhiệt mặt trời − Cải thiện chất lượng truyền động không đồng cấu trúc tách kênh trực tiếp sử dụng kalman filter để quan sát từ thông − Ứng dụng Kalman Filter cho dự báo nhiệt độ 2m từ sản phẩm mô hình HRM − Hệ thống dẫn đường quán tính INS/GPS − Sử dụng lọc Kalman kết hợp với thuật toán bám ảnh Camshift nhằm nâng cao chất lượng bám hệ thống robot tự động tìm kiếm bám bắt mục tiêu III MÔ PHỎNG BỘ LỌC KALMAN TRÊN MATLAB Cho toán sau: có thông số đưa từ cảm biến, nhiên thông số chịu tác động nhiễu (nguồn từ nhiễu trình, nhiễu đo lường, nhiễu Lớp: M14CQTE02-B Page 25 Tìm hiểu lọc Kalman nguồn điện cung cấp, ADC, …) Ta thiết kế lọc kalman để ước lượng giá trị tốt giá trị cần đo Mô hình toán: xk = Axk-1 + Buk-1 + wk = xk-1 + wk Với giá trị z R zk = Hxk + vk = xk + vk Trong đó: Giá trị cần đo x(t) Tín hiệu thu từ cảm biến z(t) Nhiễu trình w(t) Nhiễu đo lường v(t) Các phương trình cho lọc kalman + Phương trình cho trình Time Update + Phương trình cho trình “Measurement Update” Một số giả thiết đưa ra: Tín hiệu cần đo giá trị điện áp x(t) hai đầu đoạn mạch, giá trị xác x(t) 12V Vì có nhiễu tác động nên giá trị đo không xác 12V mà biến đổi qua lần đo, giá trị đo có dạng hình Lớp: M14CQTE02-B Page 26 Tìm hiểu lọc Kalman Hình 4.1: Tín hiệu đọc từ cảm biến có nhiễu Với toán nhiễu hệ thống 0, nhiên để hệ thống đáp ứng với thay đổi nhạy hơn, ta cho phương sai nhỏ Giả sử w(t) (0, 1e-3) Q = 1e-3 Với nhiễu đo lường v(t) , toán mô lấy giá trị từ cảm biến ta cho nhiễu khoảng 10% giá trị thật Việc lựa chọn giá trị phương sai R nhiễu đo lường v(t) ảnh hưởng đến tốc độ ước lượng hệ thống Từ phương trình tính hệ số Kalman Kk, ta thấy Kk tỉ lệ nghịch với R, giá trị R lớn tốc độ ước lượng chậm hơn, giá trị ước lượng tin tưởng vào giá trị đo Còn với giá trị R nhỏ ngược lại, tốc độ ước lượng nhanh hơn, lọc tin tưởng vào giá trị đo Như việc lựa chọn tham số R quan trọng , lựa chọn R cứng giá trị cố định, giá trị mềm thay đổi, ví dụ thay đổi theo tốc độ thay đổi thông số cần đo chẳng hạn Dưới kết mô lọc Kalman phần mềm Matlab với tham số R khác Kết mong muốn đường màu xanh da trời, giá trị từ cảm biến đường màu đỏ, kết ước lượng lọc Kalman đường màu xanh Với R = 1: Lớp: M14CQTE02-B Page 27 Tìm hiểu lọc Kalman Hình 4.2: Kết ước lượng với tham số R = Nhận thấy tồn nhiễu lớn kết thu từ lọc gần giá trị xác Giá trị ban đầu x(0) chọn xa so với giá trị xác, trình thích nghi cho thấy tự biến đổi để đạt đến giá trị tốt Hiệp phương sai giảm dần theo thời gian cho thấy kết ước lượng gần giống với giá trị thực thông số cần đo Sau khoảng 50 bước bắt đầu hội tụ 0.031, so với giá trị gán ban đầu Với R = 10: Bây tăng giá trị R lên 10 lần, K = 10, thấy lọc chậm đáp ứng hơn, nhiên đường ước lượng phẳng Điều cho thấy nên lựa chọn R lớn với hệ thống có thông số có tốc độ thay đổi chậm Giá trị P hội tụ 0.095 Hình 4.3: Kết ước lượng với tham số R = 10 Lớp: M14CQTE02-B Page 28 Tìm hiểu lọc Kalman Với R = 0.01: Bây giảm R xuống 100 lần, K = 0.1, từ kết mô ta thấy đường giá trị ước lượng không phẳng cho lắm, tốc độ đáp ứng lọc nhanh Như vậy, nên lựa chọn R nhỏ với hệ thống có thông số thay đổi nhanh Giá trị P hội tụ 0.0095 Hình 4.4: Kết ước lượng với tham số R = 0.1 Giá trị đầu vào thay đổi: Bây ta không cho đầu vào số cố định nữa, mà có thay đổi Ví dụ cho biến đổi theo hàm bậc Ta giữ nguyên mô hình hệ thống Ta thấy với giá trị R nhỏ kết ước lượng bám theo giá trị mong muốn Ví dụ với R = 0.1 Hình 4.5: Kết ước lượng đầu vào biến đổi (R = 1) Lớp: M14CQTE02-B Page 29 Tìm hiểu lọc Kalman Với R = 10 kết ước lượng không tốt vậy, đường giá trị ước lượng không bám theo đường giá trị xác Hình 4.6: Kết ước lượng đầu vào biến đổi (R = 2) Nhận xét: - Bộ lọc Kalman cho thấy có khả giải tốt với nhiễu cho mô hình toán biết trước mô hình Việc lựa chọn thông số R nhiễu đo lường quan trọng, đòi hỏi phải dung hòa tốc độ hội tụ độ phẳng kết ước lượng Lớp: M14CQTE02-B Page 30 PHỤ LỤC Chương trình mô Matlab function kalman(number) %input for i=1:number input(i) = 12; end %process noise and measurement noise w = 1e-5; %process noise, may it equal to Zero v = 1e0; %mesurement noise %equivelent covariance Q = 1e-3; R = 1; %initial value xpre = 0; %chosing this value is not importance Ppre = 1; %must different from Zero %to plot the result time = []; % timing pos = []; % measurement result, from sensor posest = []; % measurement estimate, after Kalman filter Pest = []; %loop for i=1:number %making input and output x = input(i) + w*randn; z = x + v*randn; %measurement update (correct) K = Ppre* inv(Ppre + R); xpre = xpre + K*(z - xpre); P = (1 - K)*Ppre; %update for ploting time = [time i]; pos = [pos z]; posest = [posest xpre]; Pest = [Pest P]; %time update (predict for next step) xpre = xpre; Ppre = P + Q; end clf plot(time, pos, 'O-', 'MarkerEdgeColor','r', 'Color','r') hold on plot(time, posest, 'O-', 'MarkerEdgeColor','g','LineWidth',2,'Color','g'); plot(time, input,'LineWidth',2,'MarkerEdgeColor','r','MarkerFaceColor','g','Mar kerSize',20,'Color','B'); xlabel('So lan do') ylabel('Dien ap (V)') legend('Gia tri luong', 'Gia tri uoc luong', 'Gia tri chinh xac') grid on TÀI LIỆU THAM KHẢO [1] https://sites.google.com/site/hannvntu/dhinh-huong-nghien-cuu/bo-lockalman/vi-du-bo-loc-kalman [2] https://en.wikipedia.org/wiki/Kalman_filter [3] https://tamgiang.wordpress.com/2009/12/12/xay-dung-bo-loc-kalman-dongian/ [4] https://www.scribd.com/doc/117913378/Kalman-Filter ... tốc góc (gyro) Tín hiệu đầu lọc tín hiệu inclinometer gyro loại nhiễu nhờ hai nguồn tín hiệu hỗ trợ xử lý lẫn lọc, thông qua quan hệ (vận tốc góc = đạo hàm/vi phân giá trị góc Bô lọc Kalman đơn... định, lọc Kalman cho ước lượng giá trị tối ưu (chính xác loại sai số, nhiễu) sử dụng tín hiệu “tinh khiết” có độ phân bổ không đổi Trong hệ thống này, tín hiệu cảm biến vào lọc gồm hai tín hiệu: ... thường sai số lớn Để giải vấn đề nhiễu thường sử dụng lọc thích hợp với sai số ảnh hưởng đến cảm biến Một số phương pháp lọc nhiễu thường sử dụng là: lọc trung bình, lọc thông thấp, thông cao lọc Kalman