Tài liệu Sử dung bộ lọc kalman trong bài toán bám mục tiêu

HỌC VIỆN CÔNG NGHỆ BƯU CHÍNH VIỄN THÔNG ------------------------------------ LƯU VĂN QUYỀN SỬ DỤNG BỘ LỌC KALMAN TRONG BÀI TOÁN BÁM MỤC TIÊU CHUYÊN NGÀNH: KỸ THUẬT VIỄN THÔNG Mãsố: 60.52.02.08 TÓM TẮT LUẬN VĂN THẠC SĨ HÀ NỘI - NĂM 2013   Luận văn được hoàn thành tại: HỌC VIỆN CÔNG NGHỆ BƯU CHÍNH VIỄN THÔNG Người hướng dẫn khoa học: GS.TSKH. NGUYỄN NGỌC SAN Phản biện 1:……………………………………………. Phản biện 2:……………………………………………. Luận văn sẽ được bảo vệ trước hội đồng chấm luận văn thạc sĩ tại Học viện Công nghệ Bưu chính Viễn thông Vào lúc:….giờ…..ngày….tháng….năm….. Có thể tìm hiểu luận văn tại: - Thư viện của Học viện Công nghệ Bưu chính Viễn thông 1 MỞ ĐẦU Với sự phát triển của khoa học kỹ thuật. Nhu cầu sử dụng hệ thống định vị và dẫn đường đã trở thành một nhu cầu không thể thiếu trong cuộc sống ngày nay. Từ nguồn gốc đó việc theo dõi bám sát mục tiêu hiện đại ngày nay là sự kết hợp của các khoa học kỹ thuật nhận dạng mục tiêu và phương pháp theo dõi đối tượng, bao gồm vị trí, kích thước, hình dáng vận tốc của đối tượng. Bám mục tiêu sử dụng cho nhiều mục đích khác nhau như vệ tinh giám sát không gian được sử dụng theo dõi các chuyển động của mục tiêu nhất định. Thời gian gần đây việc ứng dụng bộ lọc Kalman để ước lượng quỹ đạo của đối tượng qua các khung hình được sử dụng nhiều trong các thiết bị điện tử dân dụng như Camera giám sát, điều hướng Robot, dò tìm mìn, thiết bị kiểm tra hành lý…. Cho đến nay đã có nhiều công trình nghiên cứu về lĩnh vực bám bắt mục tiêu trên cơ sở xử lý ảnh và các thuật toán bám theo đối tượng chuyển động như: So khớp mẫu, Meanshift, Camshift, Particle, Kalman… Mỗi phương pháp có các ưu điểm và nhược điểm khác nhau và cho hiệu quả nhất định với từng loại đối tượng và mục tiêu theo dõi khác nhau. Nội dung của luận văn được cấu trúc thành các phần như sau: Chương I. Tổng quát về lý thuyết bám mục tiêu Chương II. Sử dụng bộ lọc Kalman trong bài toán bám mục tiêu Chương III. Ví dụ minh hoạ 2 CHƯƠNG I: TỔNG QUÁT VỀ LÝ THUYẾT BÁM MỤC TIÊU 1.1. Những khái niệm cơ bản. 1.1.1 Định nghĩa về bài toán bám mục tiêu Cho một đối tượng (S) có đầu ra là y(t) trước tác động của đầu vào u(t) Hình 1.1. Sơ đồ của đối tượng điều khiển với đầu vào và đầu ra Bài toán bám mục tiêu cần phải tìm tín hiệu điều khiển ở đầu vào sao cho tín hiệu ở đầu ra yˆ (t ) bám theo mục tiêu. 1.1.2 Định nghĩa về sai số Hầu như các phương pháp đánh giá, ước lượng tham số mô hình được xây dựng trên cơ sở áp dụng nguyên lý về kỹ thuật tham chiếu, trong đó xác định một hàm sai số để phản ánh sự khác lệch giữa mô hình và hệ động học thực. 1.1.2.1 Phương pháp sai số đầu ra Hàm sai số được định nghĩa: e0 (t ) = y (t ) − yˆ (t ) (1.1) 1.1.2.2 Phương pháp sai số đầu vào Trong phương pháp sai số đầu vào không sử dụng trực tiếp dữ liệu đo lường về đạo hàm các bậc theo thời gian của tín hiệu đầu vào hệ động học nên không cần phải quan tâm đến đặc tính kích thích liên tục nên bài toán ước lượng tham số mô hình nói riêng, nhận dạng hệ động học nói chung bao giờ cũng có nghiệm. 1.1.2.3 Phương pháp sai số phương trình. Sai số phương trình được định nghĩa trực tiếp từ phương trình động học của mô hình như sau: ec (t ) = Hˆ ( s ) yˆ (t ) − Kˆ ( s )u (t ) (1.2) 3 1.1.2.4 Phương pháp sai số dự báo Sai số dự báo được định nghĩa như sau: e pe (t ) = ⎤ Cˆ ( s ) ⎡ K ( s) ⎢ yˆ (t ) − H ( s ) u (t ) ⎥ ˆ D( s) ⎣ ⎦ (1.3) 1.1.3 Sử dụng tiêu chí tối ưu 1.1.3.1 Khái niệm Chỉ tiêu chất lượng J của một hệ thống có thể được đánh giá theo sai lệch của đại lượng điều khiển, thời gian quá độ hay theo một chỉ tiêu hỗn hợp trong điều kiện làm việc như hạn chế về công suất, tốc độ, gia tốc… 1.1.3.2 Tiêu chí tối ưu tác động nhanh (thời gian tối thiểu) Đối với bài toán tối ưu tác động nhanh thì chỉ tiêu chất lượng J có dạng. T J = ∫ 1dt = T (1.6) 0 1.1.3.3 Tiêu chí năng suất tối ưu. Năng suất ở đây được xác định bởi chất lượng của hệ thống bám theo mục tiêu trong thời gian T nhất định. Khi đó chỉ tiêu chất lượng J có dạng. T T 0 0 J = ∫ L[x(t ), ut ), t ]dt = ϕT − ϕ0 = ∫ φ tdt (1.7) 1.1.3.4 Tiêu chí năng lượng tối ưu. Chỉ tiêu chất lượng J đối với tiêu chi năng lượng tối thiểu có dạng. T J = ∫ u 2 (t )dt (1.10) 0 1.1.4 Xây dựng khâu phản hồi. Xét một hệ thống được mô tả bởi các phương trình đầu ra và trạng thái: ⎧ x& = Ax + Bu ⎨ ⎩ y = Cx (1.11) Chọn một luật điều khiển có dạng: u = (r − Kx) (1.12) 4 x& Hình 1.3: Điều khiển sử dụng phản hồi biến trạng thái 1.1.5 Xây dựng điều khiển bám bằng phản hồi trạng thái. Bài toán đặt ra là điều khiển đối tượng được mô tả: dx = A x + bu dt T y=c x (1.17) 0 ... 0 ⎞ ⎛ 0 ⎞ ⎛0 ⎜ ⎟ 0 ... 0 ⎟ ⎜⎜M ⎟⎟ dx ⎜ 0 = z+ u M M ⎟ ⎜0⎟ dt ⎜M ⎜ ⎟ ⎜ ⎟ ⎝ − a0 − a1 − an −1 ⎠ ⎝1 ⎠ y = (c%1 c%2 L c%n ) z (1.18) z a T Hình 1.4 Điều khiển bám với đối tượng (1.18) u ω Đối tượng (1.17) z a T S y x Hình 1.5 Điều khiển bám với đối tượng (1.17) 5 1.1.6 Bài toán tổng hợp hệ thống Bài toán tổng hợp hệ thống là toàn bộ quá trình tính toán, lựa chọn bổ sung thêm các khâu phù hợp vào hệ thống. Để hệ thống đó khi hoạt động đạt được những yêu cầu chất lượng đã đề ra về độ sai lệch, thời gian đáp ứng quá độ…. 1.2 Phương pháp bám mục tiêu truyền thống. 1.2.1 Thảo luận phương pháp Điều khiển truyền thống sử dụng thông tin, dữ liệu của tín hiệu đầu ra của đối tượng điều khiển làm tín hiệu đầu vào để đưa ra tín hiệu điều khiển đối tượng. Hệ thống này được biểu diễn bởi các phương trình sau: x& (t ) = A(t ) x(t ) + B(t )u (t ) (1.25) y (t ) = C ' (t ) x(t ) (1.26) Hình 1.8 Hệ thống điều khiển cổ điển 1.2.2 Bộ điều khiển PID: (Proportional-Integral-Derivative) Tên gọi PID là chữ viết tắt của ba thành phần cơ bản có trong bộ điều khiển (hình 1.10a) gồm khâu khuếch đại (P), khâu tích phân (I), khâu vi phân (D). 1 TI s TD s Hình 1.10 Điều khiển với bộ điều khiển PID Bộ điều khiển PID được mô tả có dạng tổng quát sau. t u (t ) = k P [e(t ) + 1 de(t ) e(τ )dτ + TD ] ∫ TI 0 dt (1.28) Từ mô hình vào – ra tổng quát ta có được hàm truyền của bộ điều khiển PID 6 R ( s ) = k P [1 + 1 + TD s] TI s (1.29) 1.2.3 Chọn tham số PID tối ưu theo sai lệch bám Bài toán có nhiệm vụ xác định các tham số của bộ điều khiển PI, gồm kp, TI trong công thức (1.29) hoặc kp, TI, TD trong công thức (1.31) sao cho tín hiệu ra y(t) “bám” được vào hiệu lệnh ω(t) một cách tốt nhất theo nghĩa. Q = ω (t ) − y (t ) 2 = e(t ) 2 (1.32) → min Và bài toán thiết kế bộ điều khiển PID tối ưu trở thành ∗ (1.34) p = arg min f ( p) 1.3. Phương pháp bám mục tiêu hiện đại. 1.3.1. Thảo luận phương pháp. Lý thuyết điều khiển hiện đại sử dụng mô tả không gian trạng thái trong miền thời gian, một mô hình toán học của một hệ thống vật lý như là một cụm đầu vào, đầu ra và các biến trạng thái quan hệ với phương trình trạng thái bậc một. Xuất phát từ quan điểm dễ triển khai đó là trường hợp luật điều khiển tuyến tính, được cho bởi: u(t) = K’(x(t),t) (1.36) Hình 1.12: Mô hình phương pháp điều khiển hiện đại. 7 1.3.2. Bộ quan sát trạng thái. u B + + x& System x ∫ C y A ŷ L + B x& + Observer ∫ x̂ C A x̂ Hình 1.14 Bộ quan sát trạng thái Luenberger bậc đầy đủ Hệ thống trong hình 1.14 được định nghĩa bởi: x& = Ax + Bu (1.38) y = Cx (1.39) 1.3.3 Điều chỉnh trạng thái (LQR) (Linear Quadratic Regulator) Khảo sát vấn đề duy trì trạng thái của hệ thống ở giá trị là 0, chống tác động nhiễu, đồng thời với cực tiểu tiêu hao năng lượng x& = Ax + Bu, x(0) = x 0 (1.43) y = Cx ∞ 1 min J = ∫ ⎡⎣ xT Qx + u T Ru ⎤⎦dt , 20 (1.44) Chọn luật điều khiển hồi tiếp trạng thái u = - Kx, K là hằng số, thay vào biểu thức của J ∞ J= 1 T x (Q + K T RK ) xdt ∫ 20 1.3.4 Giải thuật thiết kế LQG (Linear Quadratic Gausian) Giả sử phương trình đo lường ngõ ra được cho bởi. (1.45) 8 x& = Ax + Bu + γ w (1.57) y = Cx + v Giả sử phương trình hồi tiếp có trạng thái đầy đủ. u = -Kx + r (1.58) Nếu K được chọn sử dụng phương trình Riccati LQR và L được chọn bởi sử dụng phương trình Riccati của bộ lọc Kalman. Điều này được gọi là thiết kế LQG. Điều quan trọng của các kết quả này là trạng thái hồi tiếp của K và độ lợi của bộ quan sát L có thể được thiết kế riêng rẽ.[4] 1.3.5 Mô tả bộ ước lượng trạng thái giảm bậc Cho một hệ động học S tuyến tính bậc n mô tả bởi (S) x&n = An xn + Bnun (1.62) yn = Cn xn Với , bậc của phần đồng thời điều khiển và quan sát được của S, các vector và ma trận có kích thước phù hợp. Hãy xác định bộ đánh giá trạng thái (SE) bậc e, (SE) x&e = Ae xe + Beue (1.63) ye = Ce xe 1.3.6 Giảm bậc phần tử điều khiển Các bài toán liên quan đến phần tử điều khiển dựa vào tín hiệu phản hồi để làm cơ sở đề ra chiến lược điều khiển và cần phải xử lý trong khâu khép kín. Từ vị trí xuất phát của tín hiệu phản hồi mà trong lý thuyết hệ thống chia ra thành điều khiển truyền thống và điều khiển hiện đại 1.4. Vai trò của bộ lọc Kalman 1.4.1 Đặt vấn đề. Phương trình trạng thái của đối tượng x& = Ax + Bu + γ w (1.72) y = Cx + v (1.73) 9 x& ŷ x& x̂ x̂ Hình1.15 Bộ quan sát trạng thái của Kalman Phương trình trạng thái của khâu lọc Kalman: x&ˆ = Axˆ + Bu + L( y − yˆ ) yˆ = Cxˆ (1.74) Mục tiêu của thiết kế bộ lọc Kalman là tìm độ lợi ước lượng L để có sự ước lượng tối ưu trong sự hiện diện của nhiễu w(t) và v(t) Sai số ước lượng: xˆ (t ) = x(t ) − xˆ (t ) (1.75) 1.4.2 Mô hình toán học. 1.4.3 Quá trình ước lượng trạng thái. Quá trình ước lượng sử dụng phương pháp mạch lọc Kalman trong giám sát bám mục tiêu dược chia thành hai giai đoạn. 10 ˆx − = F ˆx k − 1 K k = Pk− − H T ( HPk− H T + R )−1 Pk − = FPk −1 F T + Q ˆxk = ˆxk− + K k ( zk − Hx ˆ k− ) Pk = ( 1 − K k H )Pk− Thực chất của giải thuật Kalman tuyến tính là một phương pháp ước tính đệ quy tuyến tính cho phép ước lượng trạng thái của một hệ thống có nhiễu sao cho độ lệch giữa giá trị ước lượng và giá trị thực tế là bé nhất. 1.4.4 Vai trò của bộ lọc Kalman Lọc Kalman nhằm ước lượng giá trị đích thực của một cái gì đó, bằng cách dự đoán giá trị của nó và tính độ tin cậy (hay độ bất định) của dự đoán đó, đồng thời đo đạc giá trị (nhưng bị sai số vì có các nhiễu), sau đó lấy một trung bình có trọng giữa giá trị dự đoán và giá trị đo đạc được, làm giá trị ước lượng. Có thể coi nó là một trường hợp của “suy diễn có điều kiện kiểu bayes” Các thuộc tính cơ bản của bộ lọc Kalman được bắt nguồn từ các yêu cầu của ước lượng trạng thái. 1.5 Kết luận chương Trong chương này luận văn nêu ra được các khái niệm cơ bản về bài toán bám mục tiêu, trên những khái niệm cơ bản đó nêu ra được phương pháp bám mục tiêu truyền thống, phương pháp bám mục tiêu hiện đại. Từ đó tìm ra vai trò của bộ lọc Kalman trong bài toán bám mục tiêu. 11 CHƯƠNG II: SỬ DỤNG BỘ LỌC KALMAN TRONG BÀI TOÁN BÁM MỤC TIÊU 2.1. Các biến thể của bộ lọc Kalman 2.1.1. Nguyên tắc cơ bản Trong ứng dụng giám sát, bám mục tiêu di động, mạch lọc Kalman là quá trình lặp đi lặp lại bước dự đoán và hiệu chỉnh trạng thái của hệ thống [13]. Xét một hệ thống đại diện bởi một không gian trạng thái như phương trình (2.1) và (2.2). xk = Fxk-1 + vk (2.1) zk = Hxk + ek (2.2) 2.1.2. Mạch lọc Kalman tuyến tính Mạch lọc Kalman tuyến tính đưa ra một ước lượng tối ưu cho trạng thái kế -tiếp sử dụng công thức tuyến tính, giả sử các biến cố phân bố xác suất Gaussian. - Giá trị trung bình cho trạng thái kế tiếp: xˆk− = Fxˆk−−1 (2.3) - Hiệp phương sai của ước lượng kế tiếp: Pk− = FPk−−1 F T + Q (2.4) - Tính toán độ lợi mạch lọc Kalman: K k = Pk− H T ( HPk− H T + R) −1 (2.5) - Giá trị hiệu chỉnh trung bình: xˆk = xˆk− + K k ( zk − Hxˆk− ) (2.6) - Hiệu chỉnh hiệp phương sai: Pk = ( I − K k H ) Pk− (2.7) 2.1.3. Mạch lọc Kalman mở rộng Bộ lọc Kalman mở rộng thực hiện theo các bước ước lượng - Giá trị trung bình cho trạng thái kế tiếp: xˆk− = xˆk −1 (2.8) - Hiệp phương sai của ước lượng kế tiếp: Pk− = Pk−−1 + Q (2.9) 12 - Tính toán độ lợi mạch lọc Kalman: K k = Pk− ( Pk− + R) −1 (2.10) - Giá trị hiệu chỉnh trung bình: xˆk = xˆk− + K k ( zk − xˆk− ) (2.11) - Hiệu chỉnh hiệp phương sai: (2.12) Pk = ( I − K k ) Pk− 2.1.4. Mạch lọc Unscented Kalman Nguyên tắc cơ bản của Unscented Kalman là biến đổi Unscent. Về cơ bản, đây là một phương pháp tính toán thống kê một biến ngẫu nhiên sau khi biến đổi không tuyến tính. Cho biến ngẫu nhiên n chiều: xk-1 với giá trị trung bình x$ k −1 và ma trận hiệp phương sai Pk-1 Mạch lọc Unscented Kalman mô tả trạng thái với một tập hợp tối thiểu các điểm (sigma) mẫu được chọn lọc cẩn thận. 2n+1 điểm sigma được chọn xung quanh ước lượng trước đó, với n là kích thước của không gian trạng thái. Sau đó một trọng số xác suất được gán cho những điểm sigma. Tiếp theo, các điểm sigma này biến đổi bằng cách sử dụng biến đổi Unscent để đưa ra một ước lượng mới cho biến trạng thái. Biến trạng thái sau đó được hiệu chỉnh bằng cách biến đổi các điểm sigma thông qua các mô hình đo lường để tính toán độ lợi Kalman. Cuối cùng, ước lượng được hiệu chỉnh sử dụng độ lợi Kalman 2.2. Lọc Kalman trong bài toán bám mục tiêu theo phương pháp phân đoạn. 2.2.1. Thảo luận bài toán. 2.2.2 Mô hình bài toán. Đầu vào là một chuỗi các khung hình, giả định rằng không có sự thay đổi về cường độ ánh sáng và không có hiện tượng che khuất. Ta có thể viết như sau: yk(x) = yk-1(x – dk(x)) ( 2.35) Mô hình quan sát cho khung hình thứ k trở thành. gk(x) = yk(x) + nk(x) ( 2.36) 13 Cần phải ước lượng phân phối xác xuất có điều kiện kết hợp của trường vecter chuyển động dk, trường phân đoạn cường độ sk, và trường phân đoạn đối tượng (hay video) zk. Dùng luật Bayes ta có: p ( d k , sk , zk g k , g k −1 , g k +1 ) = p ( d k , sk , g k , g k −1 , g k +1 ) p ( g k , g k −1 , g k +1 ) (2.37) Mô hình mạng Bayes thể hiện sự tương tác giữa d k , sk , zk , g k , g k −1 , g k +1 dk zk gk-1, gk+1 sk gk Hình 2.1 Mô hình mạng Bayes cho bài toán phân đoạn video 2.3 Bám mục tiêu theo quy trình đồng thời. Trong phần này đề cập hệ thống giám sát mục tiêu 3D như hình 2.2 Hệ thống giám sát mục tiêu, mục tiêu theo dõi là người di chuyển trước ống kính camera, thu ảnh, lưu thành file .avi và đưa vào hệ thống nhận dạng và theo vết sử dụng từng mạch lọc Kalman để bám theo đối tượng cần theo dõi. Hình 2.2. Hệ thống bám mục tiêu Mô tả hệ thống: Sơ đồ hệ thống giám sát mục tiêu hình 2.2. Hệ thống gồm tín hiệu vào và bộ phận phát hiện, bám mục tiêu và đưa ra kết quả hiển thị. 14 Hình 2.3. Sơ đồ nhận dạng ảnh Bộ lọc Kalman được coi như bộ ước lượng trạng thái hệ thống, có cấu trúc lọc đơn giản và độ hội tụ tốt cùng với khả năng lọc nhiễu cao [9]. Mô hình cần được ước lượng dự báo được mô tả bởi hệ phương trình trạng thái : xk = Fxk-1 + vk zk = Hxk + ek Vector trạng thái xk=[x, y, vx , vy], vector đo lường zk = [x, y]T, ứng với tọa độ và vận tốc của ảnh đối tượng trên mặt phẳng ảnh ở thời điểm k. vk, ek là vector nhiễu trong quá trình chuyển động và sai số phép đo. 2.4. Kết luận chương Trong chương này luận văn nêu tổng về các biến thể của bộ lọc Kalman Ứng dụng thuật toán mạch lọc Kalman trong bài toán bám mục tiêu theo phương pháp phân đoạn, bám mục tiêu theo quy trình đồng thời. Đưa ra giải pháp ứng dụng thuật toán lọc Kalman theo vết đối tượng, từ file video thực hiện từng bộ lọc Kalman để bám theo người di chuyển. 15 CHƯƠNG 3: VÍ DỤ MINH HOẠ 3.1. Bài toán bám mục tiêu 3.1.1 Đặt vấn đề. Một hệ thống bám mục tiêu bằng hình ảnh là một tập hợp các bài toán nhỏ. Đầu vào của hệ thống sẽ là hình ảnh thu được tại các điểm quan sát. Đầu ra của hệ thống sẽ là thông tin về chuyển động của các đối tượng được giám sát Mô hình khái quát chung cho hệ thống bám mục tiêu. Hình 3.1 Hệ thống bám mục tiêu tổng quát 3.1.2 Bài toán phát hiện đối tượng chuyển động Đầu vào của bài toán phát hiện đối tượng chuyển động là các khung hình video thu được từ các điểm quan sát, theo dõi. Như vậy để có thể giải quyết bài toán này ta cần nghiên cứu một số đặc điểm của video. 3.1.2.1 Các khái niệm cơ bản về video. 3.1.2.2 Một số thuộc tính đặc trưng của video 3.1.3 Bài toán phân loại đối tượng 3.1.3.1 Phân loại dựa trên hình dạng. 3.1.3.2 Phân loại dựa trên chuyển động. 3.1.4 Bài toán theo vết đối tượng 3.1.4.1 Đặt vấn đề Đầu vào của bài toán theo vết đối tượng là các vết đối tượng, các đặc trưng của đối tượng đã được phát hiện thông qua khối xử lý phát hiện đối tượng, phân loại đối tượng. Như vậy nhiệm vụ của vấn đề theo vết đối tượng là chính xác hóa sự tương 16 ứng của các vết đối tượng trong các khung hình liên tiếp từ đó dự đoán hướng chuyển động của đối tượng. 3.1.4.2 Các vấn đề giải quyết - Theo vết mục tiêu dựa trên mô hình - Theo vết mục tiêu dựa trên miền. - Theo vết mục tiêu dựa tên đường viền - Theo vết mục tiêu dựa vào đặc trưng * Chính xác hoá đối tượng tương ứng (Object matching): * Dự đoán chuyển động Nếu giải quyết bài toán bám theo mục tiêu đạt hiệu quả và độ tin cậy cao, có thể ứng dụng trong rất nhiều lĩnh vực. 3.2 Chương trình mô phỏng bám mục tiêu 3.2.1 Qúa trình thu nhận và nhận dạng ảnh Mô hình hệ thống Camera giám sát mục tiêu: Quá trình ghi hình được thực hiện bằng Webcam của máy Laptop thông qua chức năng hỗ trợ Image Acquistion của phần mềm matlab và lưu lại với dạng .avi hoặc .mat. Sau đó sử dụng file này input cho module nhận dạng ảnh và bám theo vết mục tiêu được thực hiện bằng bộ lọc Kalman Chương trình mô phỏng quá trình nhận dạng và bám mục tiêu thực hiện theo lưu đồ hình 3.6. 17 3.2.2. Bám mục tiêu sử dụng thuật toán Kalman Sau khi mục tiêu được nhận dạng, phát hiện chuyển động từ rút trích đặc trưng sẽ được thuật toán Kalman bám theo vết đối tượng thực hiện theo lưu đồ hình 3.7. 18 Rút trích đặc trưng Dự đoán Tính độ lợi Kalman Hiệu chỉnh Kết thúc N Y Hiển thị Hình 3.7 Lưu đồ thuật toán lọc Kalman 3.2.2.1 Thuật toán mạch lọc Kalman tuyến tính Bước dự đoán: − x$ k = F x$ k −1 , (3.1) Pk− = FPk −1 F T + Q . Độ lợi Kalman: K k = Pk− H T ( HPk− H T + R )−1 . (3.2) Bước hiệu chỉnh: − − x$ k = x$ k + K k ( zk − H x$ k ) , Pk = ( I − K k H )Pk− . 3.2.2.2 Thuật toán mạch lọc Kalman mở rộng (3.3)