BÁO CÁO MÔN HỌC THIẾT KẾ LỌC SỐ VÀ MÃ HÓA BĂNG CON

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA --- oOo --- BÁO CÁO MÔN HỌC THIẾT KẾ LỌC SỐ VÀ MÃ HÓA BĂNG CON Đề tài: BỘ LỌC KALMAN VÀ ỨNG DỤNG Giảng viên hướng dẫn : TS. NGÔ VĂN SỸ Học viên thực hiện : NGUYỄN TRÍ PHƯỚC DƯƠNG NGỌC PHÁP TRẦN NGUYÊN ĐỘ Lớp : K25.KDT.DN Khóa học : 04/2012 – 04/2014 Đà nẵng, tháng 3/2013 MỤC LỤC 1. Giới thiệu chủ đề nghiên cứu .......................................................................... 1 1.1 Giới thiệu về bộ lọc Kalman...................................................................... 1 1.2 Các mô hình ứng dụng bộ lọc Kalman ...................................................... 1 1.2.1 Hệ thống định vị Robot .......................................................................... 1 1.2.2 Hệ thống bám đuổi Radar ....................................................................... 1 1.2.3 Hệ thống lái tự động Autopilot ............................................................... 1 2. Cơ sở lý thuyết................................................................................................. 2 2.1 Lý thuyết xác suất ...................................................................................... 2 2.1.1 Định nghĩa xác suất ................................................................................ 2 2.1.2 Các tiên đề xác suất ................................................................................ 2 2.1.3 Xác suất có điều kiện .............................................................................. 2 2.1.4 Tính độc lập xác suất .............................................................................. 2 2.1.5 Quy tắc Bayes ......................................................................................... 3 2.2 Khái quát về bộ lọc Kalman ...................................................................... 3 2.3 Nguyên lí hoạt động của bộ lọc Kalman ................................................... 4 2.3.1 Nguyên lí ................................................................................................ 4 2.3.2 Bản chất thống kê của bộ lọc Kalman .................................................... 7 2.4 Xây dựng phương trình vi phân của bộ lọc ............................................... 7 2.5 Điều kiện dừng của luật điều chỉnh ......................................................... 11 2.6 Bộ lọc Kalman mở rộng .......................................................................... 12 2.7 Mô hình hệ thống và thông số ................................................................. 15 2.8 Các bước thiết kế bộ lọc Kalman ............................................................ 16 3. Tổ chức chương trình, thiết kế và mô phỏng ................................................ 17 3.1 Thuật toán bộ lọc Kalman ....................................................................... 17 3.1.1 Bộ lọc Kalman tuyến tính ..................................................................... 17 3.1.2 Bộ lọc Kalman phi tuyến ...................................................................... 18 3.2 Dự đoán ................................................................................................... 18 3.2.1 Bộ lọc Kalman tuyến tính ..................................................................... 18 3.2.2 Bộ lọc Kalman phi tuyến ...................................................................... 19 3.3 Hiệu chỉnh ............................................................................................... 19 3.3.1 Bộ lọc Kalman tuyến tính ..................................................................... 19 3.2.2 Bộ lọc Kalman phi tuyến ...................................................................... 19 3.4 Giá trị trả về ............................................................................................. 19 3.5 Chương trình mô phỏng từng ứng dụng .................................................. 20 3.5.1 Hệ thống bám đuổi Radar ..................................................................... 20 3.5.2 Hệ thống định vị Robot ........................................................................ 23 3.5.3 Hệ thống lái tự động Autopilot ............................................................. 25 4. Phân tích và đánh giá kết quả ........................................................................ 28 4.1 Hệ thống bám đuổi Radar ........................................................................ 28 4.2 Hệ thống định vị Robot ........................................................................... 30 4.3 Hệ thống lái tự động Autopilot ................................................................ 31 Tài liệu tham khảo ......................................................................................... 38 PHÂN CHIA NHIỆM VỤ BÁO CÁO PHẦN RIÊNG 1. Hệ thống bám đuổi Radar ................................................................... Nguyễn Trí Phước 2. Hệ thống định vị Robot ....................................................................... Dương Ngọc Pháp 3. Hệ thống lái tự động Autopilot .............................................................. Trần Nguyên Độ PHẦN CHUNG 4. Phần lý thuyết

Chương 1: GIỚI THIỆU CHỦ ĐỀ NGHIÊN CỨU

1.1 Giới thiệu về bộ lọc Kalman

Ngày nay nền công nghệ thế giới đang phát triển với một tốc độ chóng mặt với hàng loạt các giải pháp công nghệ ra đời mỗi năm Các bộ lọc số là một trong những đề tài được quan tâm trong nhiều năm trở lại đây vì những ứng dụng rộng rãi của nó, đặc biệt là trong các chip DSP đang được sử dụng trong hầu hết các thiết bị của nhiều lĩnh vực công nghệ đặc biệt là xử lý tiếng nói trong môi trường nhiễu Bộ lọc Kalman là một trong những bộ lọc được sử dụng rộng rãi và phổ biến để loại bỏ nhiễu trong nhiều ứng dụng thực tế với những ưu điểm vượt trội và khả năng thu được tín hiệu gần đúng với tín hiệu thật nhờ phương pháp truy hồi hiệu quả Với mong muốn tìm hiểu, áp dụng và lập trình thuật toán

Kalman vào thực tế, nhóm đề xuất chọn đề tài “ BỘ LỌC KALMAN VÀ ỨNG DỤNG”

làm tiểu luận

1.2 Các mô hình ứng dụng bộ lọc Kalman

Bộ lọc Kalman được ứng dụng rất nhiều trong các lĩnh vực như: đo lường, tự động hóa, hàng không, quân sự… Trong khuôn khổ tiểu luận này nhóm sẽ trình bày ba trong số rất nhiều ứng dụng sử dụng bộ lọc Kalman đó là: Hệ thống định vị Robot (Robot Localization), hệ thống bám đuổi Radar(GPS) và hệ thống lái tự động Autopilot

1.2.1 Hệ thống định vị Robot

Trong hệ thống định vị Robot, vì các vector trạng thái và điều khiển là không tuyến tính, nên bộ lọc Kalman phi tuyến được sử dụng để ước lượng của Robot (tọa độ, góc lệch) dựa vào vector điều khiển vận tốc với quỹ đạo cho trước, vị trí đo đạc được thu về từ GPS

Sự sai lệch giữa giá trị ước lượng tiền nghiệm và giá trị tiên nghiệm được tối thiểu sử dụng cho trạng thái tiếp theo nhằm tối thiểu độ lệch này

1.2.2 Hệ thống bám đuổi Radar

Trong hệ thống bám đuổi Radar, bộ lọc Kalman có nhiệm vụ ước lượng vị trí và vận tốc của Radar dựa trên các số liệu đo được thực tế (có nhiễu tác động), sau đó đánh giá sự sai lệch này để đưa ra các thông số mô hình sau khi ước lượng để làm đầu vào cho quá trình ước lượng tiếp theo sao cho bình phương sai lệch là bé nhất

1.2.3 Hệ thống lái tự động Autopilot

Trong hệ thống Autopilot , bộ lọc Kalman có nhiệm vụ ước lượng vị trí, độ cao, vận tốc, hương bay của máy bay dựa trên các số liệu đo được thực tế (có nhiễu tác động), sau đó

đánh giá sau đó đánh giá sự sai lệch này để đưa ra các thông số mô hình sau khi ước lượng

để làm đầu vào cho quá trình ước lượng tiếp theo sao cho bình phương sai lệch là bé nhất, nhằm điều khiển bay theo đúng quỹ đạo cho trước

Chương 2: CƠ SỞ LÝ THUYẾT

2.1 Lý thuyết xác suất

2.1.1 Định nghĩa xác suất:

Xác suất của biến cố A là một số không âm, kí hiệu P(A) biểu thị khả năng xảy ra biến

cố A và được xác định như sau:

P A = m

nVới m là khả năng xảy ra biến cố A, và n là tất cả các khả năng có thể xảy ra

* aP A = a = 1, trong đó tổng lấy theo các giá trị a thuộc miền giá trị của A

2.1.3 Xác suất có điều kiện:

Xác suất điều kiện là xác suất xảy ra sự kiện A khi biết sự kiện B xảy ra Xác suất có điều kiện được tính như sau:

Tức là nếu biết giá trị của B không cho ta thêm thông tin gì về B Tương tự với công thức trên, khi A độc lập với B ta có:

P A, B = P A P B và P B\A = P(B)

Mở rộng cho trường hợp xác suất có điều kiện, ta nói rằng A độc lập có điều kiện với

B khi biết C nếu:

P A\B, C = P A\C hoặc P B\A, C = P(B\C) Khi đó ta có

P A, B\C = P A C P B\C

Ý nghĩa: nếu đã biết giá trị của C thì việc biết giá trị của B không cho ta thêm thông tin

về A

2.1.5 Quy tắc Bayes

Quy tắc Bayes có dạng sau:

P A\B = P B\A P(A)

P(B)Quy tắc Bayes đóng vai trò quan trọng trong suy diễn xác suất Thông thường ta đòi hỏi tính P(A\B) tuy nhiên trong nhiều trường hợp việc tính P(B\A) có thể dễ dàng hơn Khi

đó quy tắc Bayes cho phép ta quy việc tính P(A\B) về tính P(B\A)

2.2 Khái quát về bộ lọc Kalman

Năm 1960 R.E Kalman đã xuất bản một bài báo với tiêu đề “A New Approach to Linear Filtering and Predication Problems” nhằm khắc phục hạn chế của bộ lọc Weiner-Hopf trong việc giải quyết bài toán thống kê tự nhiên Kể từ đó, danh từ bộ lọc Kalman đã

ra đời Bộ lọc này ước lượng trạng thái n

x  của quá trình thời gian rời rạc theo phương trình sai phân tuyến tính:

1 1

z   (2.2)

Biến ngẫu nhiên wk và vk biểu diễn nhiễu đo và nhiễu quá trình Trong thuật toán lọc Kalman, đặc tính thống kê của hai biến này phải được biết trước Chúng ta giả sử các biến này độc lập có phổ trắng và phân bố Gauss

u  Ma trận H(mxn) trong phương trình (2.2) là ma trận đo lường Trong thực tế H có thể thay đổi theo từng thời điểm, ở đây chúng ta giả sử là hằng

số

Một cách khái quát, bộ lọc Kalman là một tập hợp các phương trình toán học mô tả một phương pháp tính toán truy hồi hiệu quả cho phép ước đoán trạng thái của một quá trình sao cho trung bình phương sai của độ lệch (giữa giá trị thực và giá trị ước đoán) là nhỏ nhất

Bộ lọc Kalman rất hiệu quả trong việc ước đoán trạng thái trong quá khứ, hiện tại và tương lai thậm chí ngay cả khi tính chính xác của hệ thống mô phỏng không được khẳng định

2.3 Nguyên lý hoạt động của bộ lọc Kalman

nghĩa các lỗi tiền ước lượng và lỗi hậu ước lượng như sau:

][e k e k E

R H HP

H P R

H HP H P

k

T k T

k

T k

H lại đạt độ chính xác hơn

Nói tóm lại, Bộ lọc Kalman ước lượng một quá trình bằng việc sử dụng một dạng của điều khiển phản hồi: bộ lọc ước lượng trạng thái quá trình tại một vài thời điểm và sau đó quan sát phản hồi trong dạng của nhiễu đo Các phương trình trong bộ lọc Kalman chia thành hai nhóm: các phương trình cập nhật thời gian và các phương trình cập nhật đo Các phương trình cập nhật thời gian có nhiệm vụ dự đoán trước trạng thái hiện tại và hiệp phương sai lỗi ước lượng cho thời điểm tiếp theo Các phương trình cập nhật đo có nhiệm vụ phản hồi, ví dụ việc kết hợp chặt chẽ giá trị đo mới vào giá trị tiền ước lượng để thu được những cải tiến trong giá trị hậu ước lượng

Phương trình cập nhật thời gian có thể gọi là phương trình tiên đoán, trong khi phương trình cập nhật đo có thể coi như phương trình của bộ sửa sai

Hình 2.1: Thuật toán lọc Kalman

Các phương trình cập nhật thời gian của bộ lọc Kalman rời rạc

AP

P k  k T 

1

Cập nhật thời gian (tiên đoán)

Cập nhật đo (sửa sai)

Các phương trình cập nhật đo của bộ lọc Kalman rời rạc:

2.3.2 Bản chất thống kê của bộ lọc Kalman

Công thức (2.8) thể hiện bản chất thống kê của tiền ước lượng xk quy định trên tất cả các giá trị đo trước zk (Quy tắc Bayes)

k

x E





][

k

T k k

2.4 Xây dựng phương trình vi phân của bộ lọc

Phân tích hệ thống tuyến tính, được mô tả bằng phương trình vi phân

) ( ) (

) ( ) ( )

(

t CX t Y

t Bu t AX t X

Coi tác động nhiễu W(t) và V(t) bằng không khi điều kiện đầu X(t0)=X0,

Trong đó: A(t), B(t), C(t) - ma trận hàm thời gian có kích thước (nxn), (nxm), (nxp); X(t) - Véc tơ trạng thái n chiều;

u(t) - Véc tơ điều khiển m chiều;

Y(t) - Véc tơ đầu ra hay tham số quan sát được p chiều;

Bài toán tổng hợp bộ điều chỉnh chính là tìm véctơ điều khiển u(t), đảm bảo để trong khoảng chu kỳ T hệ thống chuyển từ trạng thái ban đầu, được đặc trưng bởi điều kiện ban đầu khác không X(t0), đến trạng thái cuối X(t1)

Trong tiêu chuẩn chất lượng tối ưu hay chỉ tiêu chất lượng điều chỉnh sử dụng tiêu chuẩn trung bình bình phương

T T

T

)X(t)G(

)(tXdtX(t)Q(t)(t)Xu(t)R(t)(t)u

Như vậy, giá trị của hàm J phụ thuộc vào trạng thái ban đầu X(t0) tại thời điểm t0 và điều khiển u(t) trong khoảng (t0, t1) Mục đích của hàm trung bình bình phương này có thể được giải thích như sau:

u - Số lượng đơn vị năng lượng sử dụng để điều khiển, với R(t) - Xác định dương, đối xứng với mọi t;

X - Đơn vị chuẩn X (t) của véc tơ X(t) trong quá trình điều chỉnh, với Q(t) - Xác định dương, đối xứng với mọi t;

- Biểu thức XT(t1) G(t) X(t1) biểu thị giá trị cho phép của véc tơ X(t) ở khoảng mút điều chỉnh, với G(t) - Xác định không âm với mọi t

Tóm lại, khi điều chỉnh cần phải hướng tới mong muốn để ba giá trị trên có thể nhỏ nhất Bởi vì, bài toán điều chỉnh tối ưu là hàm tối thiểu (4.2)

Xây dựng bài toán điều chỉnh tuyến tính tối ưu có thể tiến hành như sau: tìm véc tơ điều khiển tối ưu u*

(t), với t  (t0, t1), chú ý đến hàm tối thiểu J[X(t0), u(t), t0], cũng như giá trị của J*[X(t0), t0] cũng nhỏ nhất với điều kiện phương trình của đối tượng có dạng (4.1) Bài toán điều chỉnh, được giải theo tiêu chuẩn tối ưu toàn phương tuyến tính, chúng giải trong lớp hệ thống tuyến tính trong tiêu chuẩn toàn phương Để sử dụng phương trình Hamilton-Jakobi:

    fX(t),u(t),t

t

Jt),t(u),t(XFmint

X(t ),u,t  FX( ),u( ), d GX(t )

t 0 0

)t(

Trong trường hợp của tiêu chuẩn toàn phương (4.2)

F(X,u,t)=uTRu+XTQX Giá trị tối ưu của chỉ tiêu chất lượng điều khiển J*[X(t),t] tương tự như tích phân biểu thức trong hàm (4.2), hàm này có thể xác định trong công thức toàn phương: J*[X(t),t] =

XTPX

XX

X

PXBRuRPXBRuminX

P

X

T T

1 T

T 1 T

T 1 u

Đây là phương trình vi phân tuyến tính ma trận với hệ số thay đổi gọi là phương trình Riccati

Điều kiện biên của phương trình (4.5) được suy ra trực tiếp từ điều kiện biên của phương trình Hamilton-Jakobi, trong trường hợp này áp dụng dạng:

J*X(t1),t1XT(t1)P(t1)X(t1)hayXT(t1)P(t1)X(t1)XT(t1)GX(t1) (4.6)

Bởi vì P và G đối xứng, còn X(t1) bất kỳ thì: P(t1) = G (4.7)

Có thể chỉ ra rằng phương trình (4.5) có nghiệm ổn định duy nhất với điều kiện:

- Đối tượng điều chỉnh, đặc trưng bởi ma trận A và B là điều khiển được;

- Các ma trận đối xứng Q và R, khi đưa đến tiêu chuẩn chất lượng điều chỉnh (4.2) tương ứng là xác định không âm và xác định dương;

Tất cả các điều kiện được thỏa mãn khi giải bài toán cấu trúc bộ điều chỉnh, đại lượng điều chỉnh sẽ đạt tới giá trị hằng số Tuy nhiên điều đó không phải bao giờ cũng thực hiện được

Như vậy, nếu đối tượng điều chỉnh thỏa mãn phương trình (4.1), còn tiêu chuẩn chất lượng có dạng (4.2), thì luật điều chỉnh xác định theo công thức:

u*( t )   R1( t ) BT( t ) P ( t , t ) X ( t ) (4.8)

Còn giá trị tối ưu của tiêu chuẩn chất lượng:

)(),()

*

t X t t P t X

trong đó P(t,t1) là nghiệm của phương trình Riccati với điều kiện biên (4.6)

Ta thấy rằng, yêu cầu của ma trận xác định dương R(t) có giá trị tồn tại, bởi vậy, giả thiết rằng ma trận R(t) bằng không Khi đó biểu thức uTRu trong hàm chất lượng điều khiển

sẽ tiến đến không, còn ma trận nghịch đảo R-1, khi đưa vào trong biểu thức (4.8) điều khiển tối ưu u*(t) sẽ tiến tới vô cùng Điều này là không mong muốn, bởi vì điều khiển này không thực hiện được Bởi vậy, biểu thức uTRu đưa vào trong tiêu chuẩn (4.2) được đặc trưng chi phí điều khiển

Chúng ta rút ra một số kết luận về luật điều khiển (4.8), được trình bày sơ bộ dưới dạng: * ( ) ( ) ( )

t X t K t

u   với ( ) 1 ( ) ( ) ( )

t P t B t R t

K   T Luật điều chỉnh (4.8) được phát biểu như sau:

- Đối với những hệ thống có tiêu chuẩn chất lượng toàn phương là luật tuyến tính, được xác định bằng điều khiển tối ưu u*(t) là hàm đối với véc tơ trạng thái X(t);

- Hệ thống điều chỉnh là hệ thống có liên hệ ngược, bởi vì nó có mối liên hệ giữa X và

u Kết luận này có lợi hơn khi cấu trúc bộ điều khiển so với các kết quả thu được khi giải bài toán bằng phương pháp nguyên lý cực đại Theo nguyên lý cực đại thì điều khiển tối ưu tìm được là hàm thời gian

- Bộ điều chỉnh, được thực hiện bằng điều chỉnh không có bộ nhớ (không phần tử động học), bởi vì đầu ra của bộ điều chỉnh hay đầu vào của đối tượng u trong thời điểm t được xác định bởi giá trị vào hay đầu ra của đối tượng X cũng tại thời điểm t, có nghĩa là không

có trễ

- Từ phương trình Riccati (4.5) và phương trình (4.6) có thể kết luận rằng, K(t) phụ thuộc vào thời gian ngay cả khi tất cả các ma trận được đưa vào (4.5), (4.6), không phụ thuộc vào thời gian, nghĩa là đối tượng không dừng

2.5 Điều kiện dừng của luật điều chỉnh và điều kiện ổn định hệ thống tối ƣu dừng

Chúng ta tìm điều kiện, khi đó ma trận hệ số khuếch đại K(t), đưa vào trong luật điều chỉnh khi T =  không phụ thuộc vào thời gian Để có điều này, cần thiết để nghiệm P của phương trình điều khiển tối ưu thoả mãn điều kiện:

P limP(t,T) lim P(t,T) const

P - được xác định bằng giải phương trình Riccati

Tóm lại, ma trận hệ số khuếch đại K trong luật điều chỉnh không phụ thuộc vào thời gian chỉ trong trường hợp nếu quá trình tối ưu được tiến hành trên phạm vi thời gian lớn, đối

tượng điều chỉnh dừng, còn các ma trận R và Q, khi đưa vào trong tiêu chuẩn tối ưu (4.2) không phụ thuộc vào thời gian

Vì vậy hệ thống dừng nghĩa là lựa chọn thời gian ban đầu để quá trình tối ưu bất kỳ, nghĩa là bất kỳ thời điểm ban đầu nào giá trị tối ưu của tiêu chuẩn J*

sẽ là giá trị duy nhất

Vì thời điểm ban đầu bất kỳ nên:

P limP(t,T) limP(0,T t) limP(t,T)

T T

ra đối với một mô hình trạng thái và đo đạc là phi tuyến?

Rất nhiều mô hình hệ thống động và cảm biến là không hoàn toàn tuyến tính Điều này có nghĩa là các hàm mô tả quá trình hệ thống và thông số đo đạc là phi tuyến, nhưng có thể coi như xấp xỉ tuyến tính với sai lệch nhỏ Vậy thay vì giả sử rằng hệ thống động tuyến tính, thì chúng ta làm việc trực tiếp với hệ thống động phi tuyến, bao gồm mô hình hệ thống phi tuyến và mô hình thông số phi tuyến

Giả sử rằng mô hình được biểu diễn bởi vector được cho bởi hàm phi tuyến sau:

Với hàm thông số là:

Trong đó 𝑤𝑘 và 𝑣𝑘 là nhiễu quá trình và nhiễu cảm biến Trong trường hợp này, 𝑥𝑘được biểu diễn bởi hàm f là hàm phi tuyến biểu diễn quan hệ giữa trạng thái quá trình ở thời

điểm k-1 với thời điểm k Thông số điều khiển 𝑢𝑘 và nhiễu quá trình 𝑤𝑘 với hàm trọng bằng

0 Còn giá trị cảm biến được biểu diễn bởi hàm h là hàm phi tuyến biểu diễn quan hệ giữa trạng thái quá trình 𝑥𝑘 và thông số 𝑧𝑘

Trong thực tế chúng ta sẽ không xác định được giá trị 𝑤𝑘 và 𝑣𝑘 tại từng thời điểm Tuy nhiên chúng ta có thể xấp xỉ vector trạng thái và cảm biến với việc coi nhiễu 𝑤𝑘 và 𝑣𝑘 có hàm trọng bằng 0:

Và

Với là ước lượng tiền nghiệm của quá trình ở thời điểm k

Để ước lượng quá trình sử dụng phương trình sai phân phi tuyến và quan hệ của cảm biến, ta có xấp xỉ sau:

Trong đó:

 𝑥𝑘 và 𝑧𝑘 là vector trạng thái và vector đo đạc tại thời điểm k

 và là vec tor trạng thái và vector đo đạc xấp xỉ

 là ước lượng tiền nghiệm trạng thái ở thời điểm k

 𝑤𝑘 và 𝑣𝑘 là nhiễu quá trình và nhiễu đo đạc

 Ma trận A là ma trận Jacobian của f với đạo hàm theo x, được tính bằng:

 Ma trận W là ma trận Jacobian của f với đạo hàm theo w, được tính bằng:

 Ma trận H là ma trận Jacobian của f với đạo hàm theo x, được tính bằng:

 Ma trận V là ma trận Jacobian của f với đạo hàm theo v, được tính bằng:

Ở đây chúng ta xét cho trạng thái tại thời điểm k, các ma trận A, W, H, V là thay đổi

theo từng thời điểm Sai số dự đoán mới được ký hiệu là:

Các biến ngẫu nhiên sẽ có phân bố xác suất là:

Phương trình bộ lọc Kalman được sử dụng để ước lượng là:

Bằng việc kết hợp các phương trình ở trên, ta rút ra được:

Sơ đồ mô tả hoạt động của bộ lọc Kalman mở rộng (phi tuyến) được trình bày:

Hình 2.3: Hoạt động của bộ lọc Kalman mở rộng

Chọn mô hình Kalman rời rạc hoặc mở rộng

BEGIN

Thiết lập giá trị khởi đầu:

 𝑥0là trạng thái ban đầu

 𝑃0là ma trận hiệp phương sai lỗi ban đầu

 Bước thời gian

 Tổng thời gian thực hiện quá trình

 Giá trị cảm biến sẽ đo được lấy tại các bước thời gian

Thiết lập các hệ số ma trận:

 Ma trận chuyển đổi trạng thái A

 Ma trận điều khiển quá trình B

 Vector điều khiển u

 Ma trận quan hệ cảm biến và trạng thái tiền nghiệm H

 Ma trận hiệp phương sai nhiễu quá trình Q

 Ma trận hiệp phương sai nhiễu cảm biến R



Các phương trình trạng thái tương ứng chọn mô hình:

 Bộ lọc Kalman tuyến tính (rời rạc):

 Bộ lọc Kalman phi tuyến (liên tục):

Thực hiện các phương trình cập nhật thời gian và thông số

đo đạc như sơ đồ ở trên, tương ứng với các bộ lọc Kalman lựa chọn cho mô hình để tính toán các hệ số:

, , , ,

END

Chương 3: TỔ CHỨC CHƯƠNG TRÌNH, THIẾT KẾ VÀ MÔ PHỎNG

3.1 Thuật toán bộ lọc Kalman

3.1.1 Bộ lọc Kalman tuyến tính

Hình 3.1: Thuật toán bộ lọc Kalman tuyến tính

x k Trạng thái

ước lượng được cập nhật x k

khuếch đại Kalman

3.1.2 Bộ lọc Kalman phi tuyến

Hình 3.2: Thuật toán bộ lọc Kalman phi tuyến

Q A AP

Pk  k T 

1

Giá trị dự đoán sẽ dựa vào giá trị được ước lượng và giá trị hiệp phương sai tính toán được Giá trị được ước lượng lại dựa vào các phép đo cụ thể và các giá trị khởi tạo ban đầu Sau khi dự đoán giá trị đầu tiên (dựa vào các thông số khởi tạo), hệ thống tiếp tục truy hồi

Trạng thái ước lượng được cập nhật

Z k Z k

Khởi tạo

Tính hệ số khuếch đại Kalman

Tính hệ số

Cập nhật ước lượng

Cập nhật ước lượng Tiên đoán

Cập nhật Hiệp phương sai