Kalman Filter

Lý thuyết và các ứng dụng của bộ lọc Kalman Page 1

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN CÔNG NGHỆ THÔNG TIN VÀ TRUYỀN THÔNG

----------

BÀI TẬP LỚN

XỬ LÝ TÍN HIỆU NGẪU NHIÊN

ĐỀ TÀI:

TÌM HIỂU LÝ THUYẾT VÀ CÁC ỨNG DỤNG CỦA BỘ LỌC

KALMAN

Nhóm sinh viên thực hiện:

Bùi Đình Cường 20080355

Nguyễn Khánh Hưng 20081279

Giáo viên hướng dẫn: PGS.TS Nguyễn Linh Giang

HÀ NỘI 8-2012


LỜI GIỚI THIỆU ......................................................................................................................... 3

I. LÝ THUYẾT BỘ LỌC KALMAN ...................................................................................... 4

1. Lý thuyết về ước lượng ..................................................................................................................... 4

1.1. Khái niệm .................................................................................................................................. 4

1.2. Đánh giá chất lượng .................................................................................................................. 4

1.3. Kỳ vọng (Expectation) .............................................................................................................. 5

1.4. Phương sai (Variance) ............................................................................................................... 6

1.5. Độ lệch chuẩn............................................................................................................................ 7

1.6. Hiệp phương sai (Covariance) .................................................................................................. 7

1.7. Ma trận hiệp phương sai ........................................................................................................... 8

1.8. Phân phối chuẩn (phân phối Gaussian) ..................................................................................... 8

1.9. Ước lượng của trung bình và phương sai ................................................................................ 10

1.10. Phương pháp bình phương tối thiểu .................................................................................... 11

2. Bộ lọc Kalman ................................................................................................................................ 12

2.1. Giới thiệu chung về bộ lọc Kalman ........................................................................................ 12

2.2. Mô hình toán học .................................................................................................................... 15

2.2.1. Hệ thống và mô hình quan sát ......................................................................................... 15

2.2.2. Giả thiết ........................................................................................................................... 15

2.2.3. Nguồn gốc ....................................................................................................................... 16

2.2.4. Điều kiện không chệch .................................................................................................... 17

2.2.5. Hiệp phương sai sai số .................................................................................................... 19

2.2.6. Độ lời Kalman ................................................................................................................. 20

2.2.7. Tóm tắt các phương trình của bộ lọc Kalman ................................................................. 21

II. ỨNG DỤNG CỦA BỘ LỌC KALMAN ........................................................................ 24

III. CÀI ĐẶT THỬ NGHIỆM ............................................................................................... 26

1. Tao nhiễu Gaussian ......................................................................................................................... 26

2. Cài đặt bộ lọc Kalman ..................................................................................................................... 27

2.1. Mô phỏng hoạt động của bộ lọc Kalman ................................................................................ 27

2.2. Mô phỏng hoạt động của bộ lọc Kalman mở rộng .................................................................. 31

IV. KẾT LUẬN ....................................................................................................................... 32

TÀI LIỆU THAM KHẢO .......................................................................................................... 33


LỜI GIỚI THIỆU

Ngày nay, nền công nghệ thế giới đang phát triển nhanh chóng và hàng loạt các

giải pháp công nghệ ra đời mỗi năm. Theo đó, các sinh viên ngành công nghệ ngoài việc

tiếp thu các kiến thức ở giảng đường còn phải tìm hiểu nghiên cứu thêm các công nghệ

tiên tiến trên thế giới để có thể đáp ứng được yêu cầu cao của thị trường lao động. Trong

những năm gần đây các loại cảm biến, thiết bị đo lường được sử dụng rộng rãi trong dân

dụng cũng như trong công nghiệp. Thế nhưng nhiều loại thiết bị lại rất nhạy với nhiễu,

vấn đề làm sao để loại nhiễu ra khỏi tín hiệu là một vấn đề thực sự không đơn giản.

Với những ưu điểm vượt trội, tiềm năng ứng dụng của thuật toán Kalman vào

thực tế trong việc áp dụng để lọc nhiễu trong tín hiệu là rất khả quan, vì vậy việc nghiên

cứu để năm rõ và tiến tới làm chủ phương pháp này là rất cần thiết và bổ ích. Ngoài ra

với mong muốn áp dụng và lập trình thuật toán Kalman vào thực tế nên nhóm chúng em

chọn đề tài: “TÌM HIỂU LÝ THUYẾT VÀ CÁC ỨNG DỤNG CỦA BỘ LỌC

KALMAN”.


I. LÝ THUYẾT BỘ LỌC KALMAN

Vào năm 1960, R.E Kalman đã công bố bài báo nổi tiếng về một giải pháp truy

hồi để giải quyết một bài toán lọc thông tin rời rạc truyến tính (discrete data linear

filtering). Tên đầy đủ của bài báo là “A New Approach to Linear Filtering and Prediction

Problems”. Từ đó đến nay cùng với sự phát triển của tính toán kỹ thuật số, bộ lọc Kalman

đã trở thành chủ đề nghiên cứu sôi nổi và được ứng dụng trong nhiều ngành kỹ thuật

công nghệ khác nhau: trong tự động hóa, trong định vị cũng như trong viễn thông và

trong nhiều lĩnh vực khác.

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 qủa 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à nhỏ nhất. Bộ lọc Kalman rất hiệu quả trong

việc ước đoán các 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.

1. Lý thuyết về ước lượng

1.1. Khái niệm

Trong thống kê, một ước lượng là một giá trị được tính toán từ một mẫu thử và

người ta hy vọng đó là giá trị tiêu biểu cho giá trị cần xác định trong tập hợp. Người ta

luôn tìm một ước lượng sao cho đó là ước lượng “không chệch”, hội tụ, hiệu quả và

vững(robust)..

1.2. Đánh giá chất lượng

Một ước lượng là một giá trị x được tính toán trên một mẫu được lấy một cách

ngẫu nhiên, do đó giá trị của x là một biến ngẫu nhiên với kì vọng E(x) và phương sai

V(x). Nghĩa là giá trị x có thể dao động tùy theo mẫu thử, nó có ít cơ hội để có thể bằng

đúng chính xác giá trị X mà nó đang ước lượng. Mục đích ở đây là ta muốn có thể kiểm

soát sự sai lệch giá trị x và giá trị X.

Một biến ngẫu nhiên luôn dao động xung quanh giá trị kì vọng của nó. Ta muốn là

kì vọng của x phải bằng X. Khi đó ta nói ước lượng là không chệch. Trung bình tích lũy

trong ví dụ về chiều cao trung bình của trẻ 10 tuổi một ước lượng đúng, trong khi ước


lượng về tổng số cá trong hồ được tính như trong ví dụ là một ước lượng không đúng, đó

là ước lượng thừa: trung bình tổng số cá ước lượng được luôn lớn hơn tổng số cá có thực

trong hồ.

Ta cũng muốn là khi mẫu thử càng rộng, thì sai lệch giữa x và X càng nhỏ. Khi đó

ta nói ước lượng là hội tụ. Định nghĩa theo ngôn ngữ toán học là như sau:

(xn) hội tụ nếu lim𝑛→∞

𝑝(|𝑥𝑛 − 𝑋| > 𝜀) = 0 với mọ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 hơn tiến về 0 khi kích cỡ của mẫu thử càng lớn).

Biến ngẫu nhiên dao động quanh giá trị kì vọng của nó. Nếu phương sai V(x) càng

bé, thì sự dao động càng yếu. Vì vậy ta muốn phương sai của ước lượng là nhỏ nhất có

thể. Khi đó ta nói ước lượng là hiệu quả.

Cuối cùng, trong quá trình điều tra, có thể xuất hiện một giá trị “bất thường” (ví dụ

có trẻ 10 tuổi nhưng cao 1,80 m). Ta muốn giá trị bất thường này không ảnh hưởng quá

nhiều đến giá trị ước lượng. Khi đó ta nói ước lượng là vững. Có thể thấy trung bình tích

lũy trong ví dụ về chiều cao trung bình trẻ 10 tuổi không phải là một ước lượng vững.

1.3. Kỳ vọng (Expectation)

Định nghĩa: Giả sử 𝑋 là đại lượng ngẫu nhiên rời rạc có thể nhận các giá trị

𝑥1, 𝑥2, … , 𝑥𝑛 với các xác suất tương ứng 𝑃1, 𝑃2, … , 𝑃𝑛.

Khi đó kỳ vọng của X, ký hiệu là 𝐸(𝑋) hay 𝜇 được xác định bởi công thức

𝐸(𝑋) = ∑ 𝑥𝑖𝑃𝑖

𝑛

𝑖=1

(1.1)

Nếu 𝑋 là đại lượng ngẫu nhiên liên tục có hàm mật độ xác suất là 𝑓(𝑥) thì kỳ vọng

của 𝑋 là:

𝐸(𝑋) = ∫ 𝑥𝑓(𝑥)𝑑𝑥

+∞

−∞

(1.2)

- Tính chất

i. 𝐸(𝐶) = 𝐶


ii. 𝐸(𝐶. 𝑋) = 𝐶. 𝐸(𝑋), với 𝐶 là hằng số.

iii. 𝐸(𝑋 + 𝑌) = 𝐸(𝑋) + 𝐸(𝑌)

iv. Nếu X và Y là hai đại lượng ngẫu nhiên độc lập thì:

𝐸(𝑋. 𝑌) = 𝐸(𝑋). 𝐸(𝑌)

- Ý nghĩa: Kỳ vọng của một đại lượng ngẫu nhiên chính là giá trị trung bình

(theo xác suất) của đại lượng ngẫu nhiên đó. Nó là điểm trung tâm của phân phối mà các

giá trị cụ thể của X sẽ tập trung quanh đó.

1.4. Phương sai (Variance)

Định nghĩa: Phương sai (trung bình bình phương độ lệch) của đại lượng ngẫu

nhiên X, ký hiệu 𝑉𝑎𝑟(𝑋) hay 𝑉(𝑋) được xác định bởi công thức:

𝑉𝑎𝑟(𝑋) = 𝐸[(𝑋 − 𝜇)2] (1.3)

Nếu X là đại lượng ngẫu nhiên rời rạc có thể nhận các giá trị 𝑥1, 𝑥2, … , 𝑥𝑛 với xác

xác suất tương ứng là 𝑃1, 𝑃2, … , 𝑃𝑛 thì:

𝑉𝑎𝑟(𝑋) = ∑[𝑥𝑖 − 𝜇]2𝑃𝑖

𝑛

𝑖=1

(1.4)

Nếu X là đại lượng ngẫu nhiên liên tục có hàm mật độ xác suất là 𝑓(𝑥) thì:

𝑉𝑎𝑟(𝑋) = ∫ [𝑥 − 𝜇]2𝑓(𝑥)𝑑𝑥

+∞

−∞

(1.5)

Trong thực tế ta thường tính phương sai bằng công thức:

𝑉𝑎𝑟(𝑋) = 𝐸[𝑋2] − [𝐸(𝑋)]2 (1.6)

Tính chất:

i. 𝑉𝑎𝑟(𝑋) = 0

ii. 𝑉𝑎𝑟(𝐶. 𝑋) = 𝐶2. 𝑉𝑎𝑟(𝑋);

iii. Nếu X, Y là 2 biến ngẫu nhiên độc lập thì:

𝑉𝑎𝑟(𝑋 + 𝑌) = 𝑉𝑎𝑟(𝑋 − 𝑌) = 𝑉𝑎𝑟(𝑋) + 𝑉𝑎𝑟(𝑌) (1.7)

Ý nghĩa: 𝑋 − 𝜇 là độ 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 của các giá


trị của đạ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 càng lớn thì giá trị càng phân tán và ngược lại.

1.5. Độ lệch chuẩn

Định nghĩa: Độ lệch chuẩn của đại lượng ngẫu nhiên X, ký hiệu 𝜎(𝑋) được xác

định bởi công thức:

𝜎(𝑋) = √𝑉𝑎𝑟(𝑋) (1.8)

1.6. Hiệp phương sai (Covariance)

Cho 2 biến ngẫu nhiên X và Y, ta có định nghĩa hiệp phương sai của X và Y, ký

hiêu 𝐶𝑜𝑣(𝑋, 𝑌):

𝐶𝑜𝑣(𝑋, 𝑌) = 𝐸[(𝑋 − 𝜇𝑥)(𝑌 − 𝜇𝑦)] (1.9)

trong đó 𝜇𝑥 , 𝜇𝑦 lần lượt là kỳ vọng của X, Y.

Một công thức tương đương của hiệp phương sai:

𝐶𝑜𝑣(𝑋, 𝑌) = 𝐸[𝑋𝑌] − 𝜇𝑥𝜇𝑦 (1.10)

Ý nghĩa của hiệp phương sai là sự biến thiên cùng nhau của 2 biến ngẫu nhiên:

Nếu 2 biến có xu hướng thay đổi cùng nhau (nghĩa là, khi một biến có giá trị cao hơn kỳ

vọng thì biến kia cũng có xu hướng cao hơn kỳ vọng), thì hiệp phương sai của hai biến

này có giá trị dương. Mặt khác, nếu một biến nằm trên giá trị kỳ vọng còn biến kia có xu

hướng nằm dưới giá trị kỳ vọng, thì hiệp phương sai của hai biến có giá trị âm.

Nếu 2 biến ngẫu nhiên là độc lập thì 𝐶𝑜𝑣(𝑋, 𝑌) = 0 tuy nhiên điều ngược lại

không đúng. Các biến ngẫu nhiên mà có hiêp phương sai bằng 0 được gọi là không tương

quan (uncorrelated), chúng có thể độc lập nhau hoặc không.

Như vậy nếu X, Y độc lập ta có 𝐸[𝑋𝑌] = 𝜇𝑥𝜇𝑦.

Tính chất

- 𝐶𝑜𝑣(𝑋, 𝑌) = 𝑉𝑎𝑟(𝑋)

- 𝐶𝑜𝑣(𝑋, 𝑌) = 𝐶𝑜𝑣(𝑌, 𝑋)

- 𝐶𝑜𝑣(𝑎𝑋, 𝑏𝑌) = 𝑎𝑏𝐶𝑜𝑣(𝑋, 𝑌)


- 𝐶𝑜𝑣(𝑋1 + 𝑋2, 𝑌1 + 𝑌2) = 𝐶𝑜𝑣(𝑋1, 𝑌1) + 𝐶𝑜𝑣(𝑋2, 𝑌1) + 𝐶𝑜𝑣(𝑋1, 𝑌2) +

𝐶𝑜𝑣(𝑋2, 𝑌2)

- 𝑉𝑎𝑟(𝑋 + 𝑌) = 𝑉𝑎𝑟(𝑋) + 𝑉𝑎𝑟(𝑌) + 2𝐶𝑜𝑣(𝑋, 𝑌)

1.7. Ma trận hiệp phương sai

Như chúng ta vừa trình bày, hiệp phương sai là đại lượng tính toán sự tương quan

giữa 2 biến ngẫu nhiên.

Vậy giả sử chúng ta có một vector biến ngẫu nhiên có 3 phần tử 𝑋1, 𝑋2, 𝑋3. Nếu ta

muốn tính toán sự tương quan giữa tất cả các cặp biến ngẫu nhiên thì ta phải tính tất cả 3

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 đã ra đời để cho phép ta tính tất cả

các 𝐶𝑜𝑣 giữa 2 biến ngẫu nhiên trong một vector biến ngẫu nhiên.

Cho một 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 của X, kỹ hiệu là ∑, được định nghĩa là:

∑ = [

𝐶𝑜𝑣(𝑋1, 𝑋1) 𝐶𝑜𝑣(𝑋1, 𝑋2)

𝐶𝑜𝑣(𝑋2, 𝑋1) 𝐶𝑜𝑣(𝑋2, 𝑋2)⋯

𝐶𝑜𝑣(𝑋1, 𝑋𝑛)

𝐶𝑜𝑣(𝑋2, 𝑋𝑛)⋮ ⋱ ⋮

𝐶𝑜𝑣(𝑋𝑛, 𝑋1) 𝐶𝑜𝑣(𝑋𝑛, 𝑋2) ⋯ 𝐶𝑜𝑣(𝑋𝑛, 𝑋𝑛)

]

Với 𝑋 = [ 𝑋1

⋮ 𝑋𝑛

]

Quan sát trên đường chéo của ma trận hiệp phương sai (i=j) ta thấy tại đó là các

phương sai, vì 𝐶𝑜𝑣(𝑋𝑖 , 𝑋𝑖) = 𝑉𝑎𝑟(𝑋𝑖)

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 là phân phối

chuẩn (normal distribution) hay phân phối Gaussian.

Một biến ngẫu nhiên X được gọi là có phân phối Gaussian khi nó có hàm mật độ

là hàm Gaussian, ký hiệu là 𝑋~𝒩(𝜇, 𝜎) gọi là X có phân phối chuẩn với tham số 𝜇, 𝜎.

Khi đó hàm mật độ của X là:

𝑓(𝑥; 𝜇, 𝜎) =1

𝜎√2𝜋𝑒

−(𝑥−𝜇)2

2𝜎2


Với phân phối xác suất như trên, người ta tính được 𝜇, 𝜎 lần lượt là kỳ vọng và độ

lệch chuẩn của X.

Dưới đây là đồ thị của một 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 của X là

𝑋 = 𝜇 là trục đối xứng. Độ lệch chuẩn 𝜎 (hay phương sai 𝜎2) càng lớn thì đồ thị càng

bẹt, nghĩa là các giá trị càng phân tán ra xa kỳ vọng.

Trong thực tế, các loại nhiễu trong các hệ thống đo lường có thể được mô phỏng

một cách chính xác bằng nhiễu trắng cộng. Hay nói cách khác tạp âm trắng Gaussian là

loại nhiễu phổ biến nhất trong hệ thống đo lường. Loại nhiễu này có mật độ phổ công

suất đồng đều trên miền tần số và biên độ tuân theo phân bố Gaussian. Theo phương thức

tác động thì nhiễu Gaussian là nhiễu cộng. Vậy các hệ thống đo lường phổ biến chịu tác

động của nhiễu Gaussian trắng cộng (AWGN).

Hình 1.1: Đồ thị của một số phân phối chuẩn


1.9. Ước lượng của trung bình và phương sai

Ta chọn ngẫu nhiên n cá thể trong một dân số gồm N cá thể. Ta quan tâm đến đặc

trưng định lượng Y của dân số với trung bình �̅� và phương sai V(Y). Trong mẫu đó, đặc

trưng Y có trung bình và phương sai đo được lần lượt là �̅� và 𝜎2 = 1

𝑛∑ (𝑦𝑖 − �̅�)2𝑛

𝑖=1 .

Lưu ý là các giá trị �̅� và σ2 thay đổi tùy theo mẫu thử, do đó chúng là các biến ngẫu nhiên

với trung bình và phương sai riêng khác nhau.

Ước lượng trung bình của Y:

Thông thường trung bình của Y, tức là �̅� được ước lượng bởi: �̅� = 1

𝑛∑ 𝑦𝑖

𝑛𝑖=1 ,

còn được gọi là trung bình tích lũy (hay trung bình cộng). Ta chứng minh được đây là

ước lượng không chệch (unbiased), nghĩa là 𝐸(�̅�) = 𝑌

Ước lượng phương sai của Y:

σ2 là một ước lượng của V(Y), nhưng là ước lượng không đúng, ta chứng minh

được kì vọng của σ2 luôn nhỏ hơn V(Y), tức ước lượng là thiếu.

Các ước lượng đúng của V(Y) là:

Hình 1.2: Nhiễu Gaussian


𝑛

𝑛−1𝜎2 (1.11) trong trường hợp lấy mẫu có hoàn lại

𝑁

𝑁−1

𝑛

𝑛−1𝜎2 (1.12) trong trường hợp lấy mẫu không hoàn lại

Trong trường hợp mẫu lớn, phép tính có hoàn lại và phép tính không hoàn lại là

như nhau, vì 𝑁

𝑁−1 xấp xỉ bằng 1. Vì vậy trong trường hợp tổng quát ước lượng đúng của

V(Y) là: 𝑠2 = 1

𝑛−1∑ (𝑦𝑖 − �̅�)2𝑛

𝑖=1 được gọi là phương sai tích lũy của Y.

1.10. Phương pháp bình phương tối thiểu

Trong toán học, phương pháp bình phương tối thiểu, còn gọi là bình phương nhỏ

nhất hay bình phương trung bình tối thiểu, là một phương pháp tối ưu hóa để lựa chọn

một đường khớp nhất cho một dải dữ liệu ứng với cực trị của tổng các sai số thống kê

(error) giữa đường khớp và dữ liệu.

Phương pháp này giả định các sai số (error) của phép đo đạc dữ liệu phân phối

ngẫu nhiên. Định lý Gauss-Markov chứng minh rằng kết quả thu được từ phương pháp

bình phương tối thiểu không thiên vị và sai số của việc đo đạc dữ liệu không nhất thiết

phải tuân theo, ví dụ, phân bố Gauss. Một phương pháp mở rộng từ phương pháp này là

bình phương tối thiểu có trọng số.

Phương pháp bình phương tối thiểu thường được dùng trong khớp đường cong.

Nhiều bài toán tối ưu hóa cũng được quy về việc tìm cực trị của dạng bình phương, ví dụ

như tìm cực tiểu của năng lượng hay cực đại của entropy.

Giả sử dữ liệu gồm các điểm (xi, yi) với i = 1, 2, ..., n. Chúng ta cần tìm một hàm

số f thỏa mãn:

𝑓(𝑥𝑖) ≈ 𝑦𝑖 (1.13)

Giả sử hàm f có thể thay đổi hình dạng, phụ thuộc vào một số tham số, pj với j = 1,

2, ..., m.

𝑓(𝑥) ≈ 𝑓(𝑝𝑗 , 𝑥) (1.14)

Nội dung của phương pháp là tìm giá trị của các tham số pj sao cho biểu thức sau

đạt cực tiểu:


χ2 = ∑(𝑦𝑖 − 𝑓(𝑥𝑖))2

𝑛

𝑖=1

(1.15)

Nội dung này giải thích tại sao tên của phương pháp là bình phương tối thiểu.

Đôi khi thay vì tìm giá trị nhỏ nhất của tổng bình phương, người ta có thể tìm giá

trị nhỏ nhất của bình phương trung bình:

χ2 = 1

𝑛∑(𝑦𝑖 − 𝑓(𝑥𝑖))2

𝑛

𝑖=1

(1.16)

Điều này dẫn đến tên gọi bình phương trung bình tối thiểu.

Trong hồi quy tuyến tính, người ta thay biểu thức

𝑓(𝑥𝑖) ≈ 𝑦𝑖

bằng

𝑓(𝑥𝑖) = 𝑦𝑖 + 𝜀𝑖 (1.17)

với hệ số nhiễu ε là biến ngẫu nhiên có giá trị kỳ vọng bằng 0.

Trong biểu thức của hồi quy tuyến tính x được đo chính xác, chỉ có y chịu nhiễu loạn ε.

Thêm nữa, hàm f tuyến tính với các tham số pj. Nếu f không tuyến tính với các tham số,

ta có hồi quy phi tuyến, một bài toán phức tạp hơn nhiều hồi quy tuyến tính.

2. Bộ lọc Kalman

2.1. Giới thiệu chung về bộ lọc Kalman

Được đề xuất từ năm 1960 bởi giáo sư Kalman để thu thập và kết hợp linh động

các thông tin từ cảm biến thành phần. Một khi phương trình định hướng và mẫu thống kê

nhiễu trên mỗi cảm biến được biết và xác định, bộ lọc Kalman sẽ cho ước lượng giá trị

tối ưu (chính xác do đã được loại sai số, nhiễu) như là đang sử dụng một tín hiệu “tinh

khiết” và có độ phân bổ không đổi. Trong hệ thống này, tín hiệu cảm biến vào bộ lọc

gồm hai tín hiệu: từ cảm biến góc (inclinometer) và cảm biến vận tốc góc (gyro). Tín hiệu

đầu ra của bộ lọc là tín hiệu của inclinometer và gyro đã được loại nhiễu nhờ hai nguồn

tín hiệu hỗ trợ và xử lý lẫn nhau trong bộ lọc, thông qua quan hệ (vận tốc góc = đạo

hàm/vi phân của giá trị góc.


Bô lọc Kalman đơn giản là thuật toán xử lý dữ liệu hồi quy tối ưu. Có nhiều cách

xác định tối ưu, phụ thuộc tiêu chuẩn lựa chọn trình thông số đánh giá. Nó cho thấy rằng

bộ lọc Kalman tối ưu đối với chi tiết cụ thể trong bất kỳ tiêu chuẩn có nghĩa nào. Một

khía cạnh của sự tối ưu này là bộ lọc Kalman hợp nhất tất cả thông tin được cung cấp tới

nó. Nó xử lý tất cả giá trị sẵn có, ngoại trừ độ sai số, ước lượng giá trị hiện thời của

những giá trị quan tâm, với cách sử dụng hiểu biết động học thiết bị giá trị và hệ thống,

mô tả số liệu thống kê của hệ thống nhiễu, gồm nhiễu ồn, nhiễu đo và sự không chắc

chắn trong mô hình động học, và những thông tin bất kỳ về điều kiện ban đầu của giá trị

quan tâm.

Hình 1.3: Mô hình đo lường ước lượng của bộ lọc Kalman


Hình 1.3 trên mô hình hóa hoạt động của mạch lọc Kalman. Chúng ta có tín hiệu

đo được, chúng ta có mô hình của tín hiệu đo được (đòi hỏi tuyến tính) và sau đó là áp

dụng vào trong hệ thống phương trình của mạch lọc để ước lượng trạng thái quan tâm.

Thực ra tín hiệu đo là không khó, phương trình đã có sẵn, cái chung ta cần chính là mô

hình hoá hệ thống. Để có thể ứng dụng một cách hiểu quả mạch lọc Kalman thì chúng ta

phải mô hình hóa được một cách tuyến tính sự thay đổi của trạng thái cần ước lượng hoặc

dự đoán.

Hình 1.4: Tín hiệu thu trước và sau khi lọc qua Kalman


2.2. Mô hình toán học

2.2.1. Hệ thống và mô hình quan sát

Chúng ta giả sử rằng có thể mô hình hóa bởi phương trình chuyển trạng thái

𝑥𝑘+1 = 𝑭𝑘𝑥𝑘 + 𝑮𝑘𝒖𝑘 + 𝑤𝑘 (2.1)

Trong đó 𝑥𝑘 là trạng thái tại thời điểm k, 𝒖𝑘 là vector điều khiển đầu vào, 𝑤𝑘 là hệ

thống cộng hay nhiễu quá trình – thường là nhiễu Gaussian trắng cộng (AWGN) , 𝑮𝑘 là

ma trận chuyển đổi đầu vào và 𝑭𝑘 là ma trận chuyển trạng thái.

Ngoài ra chúng ta giả sử rằng, khả năng quan sát trạng thái được thực hiện thông

qua một hệ thống đo lường có thể được biểu diễn bởi một phương trình tuyến tính như

sau

𝑧𝑘 = 𝑯𝑘𝑥𝑘 + 𝒗𝑘 (2.2)

Trong đó 𝑧𝑘 là thông tin quan sát hay đo lường thực hiện tại thời điểm 𝑘, 𝑥𝑘 là

trạng thái tại thời điểm 𝑘, 𝑯𝑘 là ma trận quan sát và 𝒗𝑘 là nhiễu cộng trong quá trình đo

lường.

2.2.2. Giả thiết

Chúng ta giả thiết như sau

Nhiễu quá trình và nhiễu đo lường 𝒘𝑘 và 𝒗𝑘 là không tương quan, là nhiễu

Gaussian trắng cộng (AWGN) có giá trị trung bình bằng không và ma trận

hiệp phương sai đã biết.

Hình 2.1: Mô hình không gian trạng thái


Khi đó,

𝐸[𝑤𝑘𝑤𝑙𝑇] = {

𝑸𝑘 𝑘 = 𝑙 0 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒,

(2.3)

𝐸[𝑣𝑘𝑣𝑙𝑇] = {

𝑹𝑘 𝑘 = 𝑙 0 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒,

(2.4)

𝐸[𝑤𝑘𝑣𝑙𝑇] = 0 𝑓𝑜𝑟 𝑎𝑙𝑙 𝑘, 𝑙 (2.5)

Trong đó 𝑸𝑘 và 𝑹𝑘 là các ma trận đối xứng nửa xác định dương.

Trạng thái khởi tạo hệ thống 𝑥0 là một vector ngẫu nhiên không tương quan

với cả hệ thống và nhiễu đo lường.

Trạng thái khởi tạo hệ thống có giá trị trung bình và ma trận hiệp phương

sai đã biết.

�̂�0|0 = 𝐸[𝑥0] 𝑣à 𝑃0|0 = 𝐸[(�̂�0|0 − 𝑥0)(�̂�0|0 − 𝑥0)𝑇] (2.6)

Đưa ra những giả định trên với mục đích để xác định, đưa ra tập giá trị quan sát

𝑧1, … , 𝑧𝑘+1, bộ lọc ước lượng ở thời điểm 𝑘 + 1 tạo ra một ước lượng tối ưu của trạng

thái 𝑥𝑘+1 mà chúng ta ký hiệu bởi �̂�𝑘+1, tối thiểu hóa kỳ vọng của hàm tổn thất bình

phương lỗi.

𝐸[‖𝑥𝑘+1 − �̂�𝑘+1‖2] = 𝐸[(𝑥𝑘+1 − �̂�𝑘+1)

𝑇(𝑥𝑘+1 − �̂�𝑘+1)] (2.7)

2.2.3. Nguồn gốc

Ký hiệu ước lượng dự đoán của trạng thái 𝑥𝑘+1 dựa trên quan sát ở thời điểm 𝑘,

𝑧1, … , 𝑧𝑘 là �̂�𝑘+1|𝑘. Đó được gọi là một bước trước dự đoán hay đơn giản là dự đoán. Bây

giờ, giải pháp để tối thiểu hóa phương trình (2.7) là kỳ vọng của trạng thái ở thời điểm

𝑘 + 1 được ước định dựa trên quan sát ở thời điểm 𝑘. Như vậy,

�̂�𝑘+1|𝑘 = 𝐸[𝑥𝑘+1|𝑧1, … , 𝑧𝑘] = 𝐸[𝑥𝑘+1|𝚭𝑘] (2.8)

Khi đó trạng thái dự đoán được cho bởi

�̂�𝑘+1|𝑘 = 𝐸[𝑥𝑘+1|𝚭𝑘]

= 𝐸[𝑭𝑘𝑥𝑘 + 𝑮𝑘𝑢𝑘 + 𝑤𝑘|𝚭𝑘]

= 𝑭𝑘𝐸[𝑥𝑘|𝚭𝑘] + 𝑮𝑘𝑢𝑘 + 𝐸[𝑤𝑘|𝚭𝑘]

= 𝑭𝑘�̂�𝑘|𝑘 + 𝑮𝑘𝑢𝑘 (2.9)

Khi sử dụng trong thực tế, nhiễu quá trình có giá trị trung bình là 0 và 𝑢𝑘 đã được

biết chính xác.


Hiệp phương sai ước lượng dự đoán 𝑷𝑘+1|𝑘 là trung bình bình phương sai số trong

ước lượng �̂�𝑘+1|𝑘.

Vì vậy, bằng việc sử dụng các sự kiện mà 𝑤𝑘 và �̂�𝑘|𝑘 là không tương quan:

𝑷𝑘+1|𝑘 = 𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘)

= 𝐸 [(𝑥𝑘+1 − �̂�𝑘+1|𝑘)(𝑥𝑘+1 − �̂�𝑘+1|𝑘)𝑇

|𝚭𝑘]

= 𝑐𝑜𝑣(𝑭𝑘(𝑥𝑘 − �̂�𝑘|𝑘) + 𝑤𝑘) = 𝑭𝑘𝑐𝑜𝑣(𝑥𝑘 − �̂�𝑘|𝑘)𝑭𝑘𝑇 + 𝐸[𝑤𝑘𝑤𝑘

𝑇]

= 𝑭𝑘𝑷𝑘|𝑘𝑭𝑘𝑇 + 𝑸𝑘 (2.10)

Đang có một ước lượng dự đoán �̂�𝑘+1|𝑘, giả sử rằng chúng ta đang có một giá trị

quan sát 𝑧𝑘+1. Làm sao để sử dụng thông tin này để cập nhật trạng thái dự đoán, tức là

tìm �̂�𝑘+1|𝑘+1. Chúng ta giả sử rằng ước lượng là tổng trọng số tuyến tính của dự đoán và

quan sát mới và có thể được mô tả bởi phương trình,

�̂�𝑘+1|𝑘+1 = 𝑲𝑘+1′ �̂�𝑘+1|𝑘 + 𝑲𝑘+1𝑧𝑘+1 (2.11)

Trong đó 𝑲𝑘+1′ và 𝑲𝑘+1 là những ma trận hiệu chỉnh bù hay ma trận độ lời (của

các kích thước khác nhau). Vấn đề của chúng ta bây giờ là tìm 𝑲𝑘+1′ và 𝑲𝑘+1 để 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 được cho bởi

�̃�𝑘+1|𝑘+1 = �̂�𝑘+1|𝑘+1 − 𝑥𝑘+1 (2.12)

2.2.4. Điều kiện không chệch

Để bộ lọc không chệch yêu cầu 𝐸[�̂�𝑘+1|𝑘+1] = 𝐸[𝑥𝑘+1]. Giả sử rằng �̂�𝑘|𝑘 là một ước

lượng không chệch. Kết hợp phương trình (2.11) và (2.12) và tính kỳ vọng

𝐸[�̂�𝑘+1|𝑘+1] = 𝐸[𝑲𝑘+1′ �̂�𝑘+1|𝑘 + 𝑲𝑘+1𝑯𝑘+1𝑥𝑘+1 + 𝑲𝑘+1𝑣𝑘+1]

= 𝑲𝑘+1′ 𝐸[�̂�𝑘+1|𝑘] + 𝑲𝑘+1𝑯𝑘+1𝐸[𝑥𝑘+1] + 𝑲𝑘+1𝐸[𝑣𝑘+1] (2.13)

Vì 𝐸[𝑣𝑘+1] = 0, và dự đoán là không chệch:

𝐸[�̂�𝑘+1|𝑘] = 𝐸[𝑭𝑘�̂�𝑘|𝑘 + 𝑮𝑘𝑢𝑘]

= 𝑭𝑘𝐸[�̂�𝑘|𝑘] + 𝑮𝑘𝑢𝑘

= 𝐸[𝑥𝑘+1] (2.14)


Do đó kết hợp phương trình (2.13) và (2.14)

𝐸[�̂�𝑘+1|𝑘+1] = (𝑲𝑘+1′ + 𝑲𝑘+1𝑯𝑘+1)𝐸[𝑥𝑘+1]

Và điều kiện để �̂�𝑘+1|𝑘+1 không chệch yêu cầu

𝑲𝑘+1′ + 𝑲𝑘+1𝑯𝑘+1 = 𝑰

ℎ𝑎𝑦 𝑲𝑘+1′ = 𝑰 − 𝑲𝑘+1𝑯𝑘+1 (2.15)

Để ước lượng không chệch yêu cầu

�̂�𝑘+1|𝑘+1 = (𝑰 − 𝑲𝑘+1𝑯𝑘+1)�̂�𝑘+1|𝑘 + 𝑲𝑘+1𝑧𝑘+1

= �̂�𝑘+1|𝑘 + 𝑲𝑘+1[𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘] (2.16)

Trong đó 𝑲 được gọi là độ lời của bộ lọc Kalman (Kalman gain).

𝑯𝑘+1�̂�𝑘+1|𝑘 có thể được hiểu như một quan sát hay đo lường dự đoán �̂�𝑘+1|𝑘

�̂�𝑘+1|𝑘 = 𝐸[𝑧𝑘+1|𝚭𝑘]

= 𝐸[𝑯𝑘+1𝑥𝑘+1 + 𝒗𝑘+1|𝚭𝑘]

= 𝑯𝑘+1�̂�𝑘+1|𝑘 (2.17)

Đặt 𝒓𝑘+1 là độ lệch đo lường thể hiện sự sai khác giữa giá trị đo lường 𝑧𝑘+1 và

ước lượng của nó �̂�𝑘+1|𝑘, được biểu diễn bởi

𝒓𝑘+1 = 𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘 (2.18)

𝐸[𝒓𝑘+1|𝚭𝑘] = 𝐸[𝑧𝑘+1 − �̂�𝑘+1|𝑘|𝚭𝑘]

= 𝐸[𝑧𝑘+1|𝚭𝑘] − �̂�𝑘+1|𝑘

= 0 (2.19)

Kết hợp (2.16) và (2.18) ta được

�̂�𝑘+1|𝑘+1 = �̂�𝑘+1|𝑘 + 𝑲𝑘+1𝒓𝑘+1 (2.20)


Hiệp phương sai độ lệch đo lường 𝑺𝑘+1 được cho bởi,

𝑺𝑘+1 = 𝐸[𝒓𝑘+1𝒓𝑘+1𝑇 ]

= 𝐸 [(𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘)(𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘)𝑇

]

𝑺𝑘+1 = 𝑯𝑘+1𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 + 𝑹𝑘+1 (2.21)

2.2.5. Hiệp phương sai sai số

Chúng ta xác định hiệp phương sai sai số dự đoán của phương trình (2.10). Bây

giờ chúng ta tính toán hiệp phương sai sai số điều chỉnh.

𝑷𝑘+1|𝑘+1 = 𝐸 [(�̃�𝑘+1|𝑘+1)(�̃�𝑘+1|𝑘+1)𝑇

|𝚭𝑘]

= 𝐸 [(𝑥𝑘+1 − �̂�𝑘+1|𝑘+1)(𝑥𝑘+1 − �̂�𝑘+1|𝑘+1)𝑇

]

= 𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘+1)

= 𝑐𝑜𝑣(𝑥𝑘+1 − (�̂�𝑘+1|𝑘 + 𝑲𝑘+1(𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘))

= 𝑐𝑜𝑣 (𝑥𝑘+1 − ( �̂�𝑘+1|𝑘 + 𝑲𝑘+1(𝑯𝑘+1𝑥𝑘+1 + 𝒗𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘)))

= 𝑐𝑜𝑣 ((𝑰 − 𝑲𝑘+1𝑯𝑘+1)(𝑥𝑘+1 − �̂�𝑘+1|𝑘) − 𝑲𝑘+1𝒗𝑘+1)

= 𝑐𝑜𝑣 ((𝑰 − 𝑲𝑘+1𝑯𝑘+1)(𝑥𝑘+1 − �̂�𝑘+1|𝑘)) + 𝑐𝑜𝑣(𝑲𝑘+1𝒗𝑘+1)

= (𝑰 − 𝑲𝑘+1𝑯𝑘+1)𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘)(𝑰 − 𝑲𝑘+1𝑯𝑘+1)𝑇 + 𝑲𝑘+1𝑐𝑜𝑣(𝒗𝑘+1)𝑲𝑘+1𝑇

𝑷𝑘+1|𝑘+1 = (𝑰 − 𝑲𝑘+1𝑯𝑘+1)𝑷𝑘+1|𝑘(𝑰 − 𝑲𝑘+1𝑯𝑘+1)𝑇 + 𝑲𝑘+1𝑹𝑘+1𝑲𝑘+1𝑇 (2.22)

Ta có thể tính toán 𝑷𝑘+1|𝑘+1 theo cách khác như sau

𝑷𝑘+1|𝑘+1 = 𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘+1)

= 𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘 − 𝑲𝑘+1𝒓𝑘+1)

= 𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘) − 𝑐𝑜𝑣(𝑲𝑘+1𝒓𝑘+1 )

= 𝑷𝑘+1|𝑘 − 𝑲𝑘+1𝑺𝑘+1𝑲𝑘+1𝑇 (2.23)


Trong đó, 𝑷𝑘+1|𝑘 = 𝑐𝑜𝑣(𝑥𝑘+1 − �̂�𝑘+1|𝑘)

𝑹𝑘+1 = 𝑐𝑜𝑣(𝒗𝑘+1) = 𝐸[𝑣𝑘+1𝑣𝑘+1𝑇 ]

Vì vậy hiệp phương sai của ước lượng điều chỉnh đã được biểu diễn qua hiệp

phương sai dự đoán 𝑷𝑘+1|𝑘, nhiễu đo lường 𝑹𝑘+1 và ma trận độ lời Kalman 𝑲𝑘+1

2.2.6. Độ lời Kalman

Mục tiêu của chúng ta là làm sao để 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 𝑲.

𝐿 = min𝑲𝑘+1

𝐸 [(�̃�𝑘+1|𝑘+1)𝑇

(�̃�𝑘+1|𝑘+1)|𝚭𝑘]

= min𝑲𝑘+1

𝑡𝑟𝑎𝑐𝑒 (𝐸 [(�̃�𝑘+1|𝑘+1)𝑇

(�̃�𝑘+1|𝑘+1)|𝚭𝑘])

= min𝑲𝑘+1

𝑡𝑟𝑎𝑐𝑒(𝑷𝑘+1|𝑘+1) (2.24)

Với bất kỳ ma trận A và ma trận đối xứng B ta có

𝜕

𝜕𝐴(𝑡𝑟𝑎𝑐𝑒(𝐴𝐵𝐴𝑇)) = 2𝐴𝐵

Kết hợp (2.23) và (2.24) và lấy vi phân ma trận độ lời và đặt kết quả bằng 0 ta

được

𝜕𝐿

𝜕𝑲𝑘+1

= −2(𝑰 − 𝑲𝑘+1𝑯𝑘+1)𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 + 2𝑲𝑘+1𝑹𝑘+1 = 0

Sắp xếp lại và đưa ra phương trình cho ma trận độ lời

𝑲𝑘+1 = 𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 [𝑯𝑘+1𝑷𝑘+1|𝑘𝑯𝑘+1

𝑇 + 𝑹𝑘+1]−1

(2.25)

Kết hợp với (2.21) ta được

𝑲𝑘+1 = 𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 𝑺𝑘+1

−1 (2.26)

Cùng với phương trình 2.16, định nghĩa một ước lượng tối ưu tuyến tính trung

bình bình phương sai số.


Từ (2.26) ta có

𝑲𝑘+1𝑺𝑘+1𝑲𝑘+1𝑇 = 𝑷𝑘+1|𝑘𝑯𝑘+1

𝑇 𝑲𝑘+1𝑇 (2.27)

Kết hợp (2.23) và (2.27)

𝑷𝑘+1|𝑘+1 = 𝑷𝑘+1|𝑘(𝑰 − 𝑯𝑘+1𝑇 𝑲𝑘+1

𝑇 )

= (𝑷𝑘+1|𝑘(𝑰 − 𝑯𝑘+1𝑇 𝑲𝑘+1

𝑇 ) )𝑇

= (𝑰 − 𝑯𝑘+1𝑇 𝑲𝑘+1

𝑇 )𝑇(𝑷𝑘+1|𝑘 )𝑇

= (𝑰 − 𝑲𝑘+1𝑯𝑘+1 )𝑷𝑘+1|𝑘 (2.28)

2.2.7. Tóm tắt các phương trình của bộ lọc Kalman

Trong phần này chúng ta sẽ tóm tắt các phương trình tổng quát của giải thuật lọc

Kalman. Giải thuật bao gồm 2 quá trình: quá trình ước lượng và quá trình điều chỉnh.

Quá trình dự đoán

Bộ lọc Kalman dựa vào trạng thái ước lượng điều chỉnh �̂�𝑘|𝑘 - là ước lượng của 𝑥𝑘

để ước lượng trạng thái �̂�𝑘+1|𝑘 – là ước lượng dự đoán của 𝑥𝑘+1 cho phép đo 𝑧𝑘+1

Trạng thái dự đoán:

�̂�𝑘+1|𝑘 = 𝑭𝑘�̂�𝑘|𝑘 + 𝑮𝑘𝑢𝑘 (2.29)

Hiệp phương sai ước lượng dự đoán:

𝑷𝑘+1|𝑘 = 𝑭𝑘𝑷𝑘|𝑘𝑭𝑘𝑇 + 𝑸𝑘 (2.30)

Đo lường dự đoán:

�̂�𝑘+1|𝑘 = 𝑯𝑘+1�̂�𝑘+1|𝑘 (2.31)


Prediction

(1) �̂�𝑘+1|𝑘 = 𝑭𝑘�̂�𝑘|𝑘 + 𝑮𝑘𝑢𝑘

(2) 𝑷𝑘+1|𝑘 = 𝑭𝑘𝑷𝑘|𝑘𝑭𝑘𝑇 + 𝑸𝑘

Update

(3) 𝑺𝑘+1 = 𝑯𝑘+1𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 + 𝑹𝑘+1

(4) 𝑲𝑘+1 = 𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 𝑺𝑘+1

−1

(5) �̂�𝑘+1|𝑘+1 = �̂�𝑘+1|𝑘 + 𝑲𝑘+1(𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘)

(6) 𝑷𝑘+1|𝑘+1 = (𝑰 − 𝑲𝑘+1𝑯𝑘+1 )𝑷𝑘+1|𝑘

Quá trình điều chỉnh

Độ lệch đo lường:

𝒓𝑘+1 = 𝑧𝑘+1 − 𝑯𝑘+1�̂�𝑘+1|𝑘 (2.32)

Hiệp phương sai độ lệch:

𝑺𝑘+1 = 𝑯𝑘+1𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 + 𝑹𝑘+1 (2.33)

Độ lời Kalman:

𝑲𝑘+1 = 𝑷𝑘+1|𝑘𝑯𝑘+1𝑇 𝑺𝑘+1

−1 (2.34)

Trạng thái ước lượng điều chỉnh:

�̂�𝑘+1|𝑘+1 = �̂�𝑘+1|𝑘 + 𝑲𝑘+1𝒓𝑘+1 (2.35)

Hiệp phương sai ước lượng điều chỉnh:

𝑷𝑘+1|𝑘+1 = (𝑰 − 𝑲𝑘+1𝑯𝑘+1 )𝑷𝑘+1|𝑘 (2.36)

Hình 2.2: Tóm tắt quá trình khởi tạo của Kalman

Initial

�̂�𝑘|𝑘 and 𝑷𝑘|𝑘


Cùng với các điều kiện ban đầu trong ước lượng và ma trận hiệp phương sai lỗi

của nó (phương trình 2.6) đã định nhĩa một giải thuật rời rạc hóa về thời gian và đệ quy

để xác định hiệp phương sai ước lượng tuyến tính tối thiểu được gọi là bộ lọc Kalman.


II. ỨNG DỤNG CỦA BỘ LỌC KALMAN

Bởi vì bộ lọc Kalman giải quyết một số vấn đề cơ bản là lọc nhiễu và tối ưu cho

các ước lượng nên nó được ứng dụng rất rộng rãi. Ngày nay Kalman được ứng dụng

nhiều trong các ô tô tự lái có khả năng thay thế con người vận hành xe, một chương trình

máy tính được cài sẵn bộ lọc Kalman sẽ có nhiệm vụ điều khiển xe. Những chiếc xe này

thậm chí còn được giới thiệu là an toàn hơn xe lái bởi con người trong một số trường hợp.

Một ứng dụng khác, có thể chúng ta không thích thú lắm, đó là các tên lửa không

đối không (air-to-air missile: AAM). Đó là các tên lửa dẫn hướng việc bắn từ mộ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ý phát hiện

mục tiêu (thông thường bằng rada hoặc hồng ngoại, đôi khi cũng sử dụng Lazer hoặc

quang học) sau đó tự động dẫn đến mục tiêu nhờ quá trình ước lượng của Kalman [3].

Ngoài ra bộ lọc Kalman còn được áp dụng nhiều vào hệ thống theo dõi mục tiêu di

động trong mạng cảm biến không dây. Do nhiễu đo lường trên các cảm biến nên kết quả

thu được thường không chính xác, có sai số lớn so với thực tế. Bộ lọc Kalman được áp

dụng để lọc nhiễu, dự đoán, ước lượng trạng thái của mục tiêu như vị trí, tốc độ và quỹ

đạo. Nhờ có quá trình dự đoán và điều chỉnh của bộ lọc Kalman đã góp phần quan trọng

vào việc quản lý trạng thái các cảm biến làm giảm thiểu năng lượng tiêu thụ cũng như

tăng chất lượng theo dõi và kéo dài thời gian sống của mạng [4].

Một số ứng dụng được liệt kê từ bài viết Kalman Filter trên Wikipedia [5]:

Lái tự động máy bay (Autopilot)

Ước lượng trạng thái sạc của pin (Battery state of charge (SoC) estimation)

Giao diện tương tác với máy tính bằng não (Brain–computer interface)

Định vị chuyển động (Dynamic positioning)

Các ứng dụng trong kinh tế, đặc biệt là kinh tế vĩ mô, time series, và econometrics

Hệ thống dẫn đường quán tính (Inertial guidance system)

Theo dõi bằng 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 3 chiều (3D-Modelling)


Ở Việt Nam có một số ứng dụng như:

Ứng dụng lọc Kalman trong phân tích biến dạng nhà cao tầng do bức xạ nhiệt mặt

trời.

Cải thiện chất lượng truyền động không đồng bộ bằng cấu trúc tách kênh trực tiếp

sử dụng kalman filter để quan sát từ thông. [6]

Ứ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. [7]

Sử dụng bộ 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 trong các hệ thống robot tự động tìm kiếm và bám bắt mục tiêu.[8]


III. CÀI ĐẶT THỬ NGHIỆM

1. Tao nhiễu Gaussian

Sử dụng chuyển đổi Box Muller để chuyển từ phân phối chuẩn trong khoảng [0; 1]

sang phân phối Gaussian 𝒩(𝜇, 𝜎2)

Giả sử biến ngẫu nhiên 𝑥~𝒩(𝜇, 𝜎2)

𝑥 = 𝜇 + 𝜎√−2 log(𝑠) cos(2𝜋𝑡) (3.1)

Hay

𝑥 = 𝜇 + 𝜎√−2 log(𝑠) sin(2𝜋𝑡) (3.2)

trong đó 𝑠, 𝑡 ∈ [0; 1]

Ví dụ tạo sinh 1000 điểm tuân theo phân phối Gaussian 𝒩(0, 1)

Trong hình vẽ trên, đồ thị bên trái là hàm mật độ xác suất và phân bố của 1000

điểm Gaussian. Đồ thị bên phải thể hiện sự thay đổi giữa phương sai và kỳ vọng. Từ hình

vẽ ta thấy phương sai càng lớn thì giá trị càng phân tán.

Hình 3.1: Tạo sinh 1000 điểm ~𝒩(0, 1)


2. Cài đặt bộ lọc Kalman

2.1. Mô phỏng hoạt động của bộ lọc Kalman

Các thông số cài đặt

Ở đây chúng ta sẽ mô hình hóa một mục tiêu chuyển động có vector trạng thái tại

thời điểm k:

𝑥𝑘 = [𝑥(𝑘) �̇�(𝑘) 𝑦(𝑘) �̇�(𝑘)]𝑇

Phương trình chuyển trạng thái của mục tiêu:

𝑥𝑘+1 = 𝑭𝑘𝑥𝑘 + 𝑤𝑘

Trong đó:

𝑭𝑘 = [

1 ∆𝑇𝑘

0 10 00 0

0 0 0 0

1 ∆𝑇𝑘

0 1

]

∆𝑇𝑘 = 𝑇𝑘+1 − 𝑇𝑘 = 1𝑠

𝑸𝑘 = 𝐸[𝑤𝑘𝑤𝑘𝑇]

𝑞: là đại lượng biểu diễn cường độ của nhiễu quá trình.

𝑥0|0 = [

1 00 000

10

] ∗ 𝑧0 ; 𝑷0|0 = 𝐼𝑛𝑑𝑒𝑛𝑡𝑖𝑡𝑦(4, 4)


Mô hình đo lường:

𝑧𝑘 = 𝑯𝑘𝑥𝑘 + 𝒗𝑘

Trong đó:

𝑯𝑘 = [10

00

01

00

]

𝒗𝑘~𝓝(0, 1)

𝑹𝑘 = 𝑬[𝒗𝑘𝒗𝑘𝑇] = 𝜎2 ∗ [

1 00 1

]

𝜎 = 5: là đại lượng biểu diễn cường độ của nhiễu đo lường.

Kết quả cài đặt

- Ngôn ngữ sử dụng C#

- Input: Quỹ đạo chuyển động của mục tiêu được vẽ bằng con trỏ chuột được

thêm nhiễu Gaussian.

- Output: Quỹ đạo đã được ước lượng.

Giao diện


Sử dụng

- Dùng con trỏ chuột vẽ một quỹ đạo chuyển động bất kỳ của mục tiêu và được

biểu diễn bởi đường màu đen.

- Khi nhấn Draw chương trình sẽ tạo dữ liệu đo lường thể hiện quá trình đo

lường vị trí của mục tiêu được thể hiện bởi đường màu đỏ.

- Bộ lọc Kalman sẽ cho kết quả là đường màu xanh chính là dữ liệu đã được ước

lượng tối ưu.

- Chúng ta có thể thay đổi các thông số để đạt kết quả tối ưu.

Kết quả

Quỹ đạo thực của mục tiêu:


Quỹ đạo đo lường

Quỹ đạo sau khi qua lọc Kalman


2.2. Mô phỏng hoạt động của bộ lọc Kalman mở rộng

Các thông số cài đặt


IV. KẾT LUẬN

Về lý thuyết

Nắm rõ được bộ lọc Kalman

Có khả năng phát triển hệ thống ước lượng dùng bộ lọc Kalman

Hiểu được cách mô hình hóa hệ thống trong bộ lọc Kalman

Về ứng dụng

Bằng những kiến thức về bộ lọc Kalman đã mô phỏng được quá trình ước lượng

chuyển động của mục tiêu.

Hướng phát triển

Ứng dụng bộ lọc Kalman vào hệ thống bắt mục tiêu trong mạng cảm biến không

dây, giúp quản lý trạng thái của các cảm biến, tăng chất lượng theo dõi và kéo

dài thời gian sống của mạng.


TÀI LIỆU THAM KHẢO

[1] Giáo trình xác suất thống kê, Tống Đình Quỳ, NXB Giáo dục, Hà Nội, 1999.

[2] Estimation II, Ian Ried-2001

[3] Tên lửa không đối không, http://vi.wikipedia.org/wiki/Tên_lửa_không_đối_không

[4] Vinh Tran-Quang, Phat Nguyen Huu, Takumi Miyoshi, “A Collaborative Target

Tracking Algorithm Considering Energy Constraint in WSNs” 15-17 Sept. 2011.

[5] Kalman filter, http://en.wikipedia.org/wiki/Kalman_filter

[6] Ths. Đinh Anh Tuấn, Nguyễn Phùng Quang, “Cải thiện chất lượng truyền động không

đồng bộ bằng cấu trúc tách kênh trực tiếp sử dụng kalman filter để quan sát từ thông” -

20-11-2009

[7] http://www.nchmf.gov.vn/web/vi-VN/71/155/5760/Default.aspx

[8] Ngô Mạnh Tiến, Phan Xuân Minh, Hà Thị Kim Duyên, “A Method using Kalman

Filter combining with Image Tracking Camshift Algorithm to bring higher tracking

Quality in automatically searching and tracking target Robot System” –VCCA-2011.

http://en.wikipedia.org/wiki/Kalman_filter

http://www.nchmf.gov.vn/web/vi-VN/71/155/5760/Default.aspx

Documents

Kalman Filter