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

Bộ lọc Kalman và ứng dụng Page 1

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 đó
Bộ lọc Kalman và ứng dụng Page 2

đá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.





















Bộ lọc Kalman và ứng dụng Page 3

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
n

Vớ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.
2.1.2 Các tiên đề xác suất
- 0  P(A = a)  1 với mọi giá trị trong miền xác định của biến A
- P(true) = 1; P(false) = 0.
- P (A v B) = P(A) + P(B) nếu A và B loại trừ tương hỗ
- Các tính chất:
* P (~A) = 1 – P(A)
* P (A) = P(A ^ B) + P(A ^ ~B)
* P (A v B) = P (A) + P (B) – P (A ^ B)
*

P

A = a


= 1
a
, 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:
P

A\B

=
P(A, B)
P(B)

2.1.4 Tính độc lập xác suất:
Ta định nghĩa, sự kiện A độc lập về xác suất với sự kiện B nếu như:
P

A\B

= P(A)
Bộ lọc Kalman và ứng dụng Page 4

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:
111 

kkkk
wBuAxx
(2.1)
Với việc đo
n
z 

kkk
vHxz 
(2.2)
Bộ lọc Kalman và ứng dụng Page 5

Biến ngẫu nhiên w
k
và v
k
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.
P(W))N(0,Q) (2.3)
P(R)N(0,R)
Trong thực tế, ma trận hiệp phương sai nhiễu quá trình Q và ma trận hiệp phương sai
nhiễu đo R phải thay đổi theo từng thời điểm, tuy nhiên chúng ta có thể giả sử là hằng số.
Ma trận A(nxn) trong phương trình sai phân (2.1) là ma trận chuyển trạng thái từ thời
điểm trước (k-1) sang thời điểm hiện tại (k). Chú ý rằng, trong thực tế A có thể thay đổi theo
từng thời điểm. Nhưng chúng ta cũng có thể giả sử nó là hằng số. Ma trận B (nx1) là ma
trận điều khiển có lối vào
n
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
2.3.1 Nguyên lý
Chúng ta định nghĩa
n
k
x 


là trạng thái tiền ước lượng ở thời điểm thứ k,
n

k
x 

là
trạng thái hậu ước lượng tại thời điểm thứ k và cho ra giá trị đo z
k
. Chúng ta có thể định
nghĩa các lỗi tiền ước lượng và lỗi hậu ước lượng như sau:




k
kk
xxe
(2.4)
k
kk
xxe


(2.5)


Ma trận hiệp phương sai lỗi tiền ước lượng:
Bộ lọc Kalman và ứng dụng Page 6

][
T
kk

eeEP


(2.6)
Ma trận hiệp phương sai lỗi hậu ước lượng:
][
T
kkk
eeEP 
(2.7)
Xuất phát từ phương trình cho bộ lọc Kalman, chúng ta đi tìm ra một phương trình tính
toán trạng thái hậu ước lượng
k
x

như là một tổ hợp tuyến tính của trạng thái tiền ước lượng


k
x
và sự khác nhau giữa giá trị đo thực tế z
k
và giá trị tiên đoán
k
xH

được chỉ trong
phương trình sau.

)(






k
k
kk
xHzKxx
(2.8)
Giá trị
)(



k
k
xHz
trong công thức (2.8) được gọi là giá trị sai khác giữa giá trị tiên đoán


k
xH
và giá trị thực tế z
k
đo được. Giá trị này bằng 0 nghĩa là hai giá trị hoàn toàn đồng
nhất với nhau.
Ma trận K (mxn) trong phương trình (2.8) gọi là hệ số khuếch đại Kalman nhằm mục
đích tối thiểu hoá hiệp phương sai lỗi hậu ước lượng (2.7). Độ khuếch đại Kalman có thể
được xác định bởi phương trình sau:

RHHP
HP
RHHPHPK
T
k
T
k
T
k
T
kk




 1
)(
(2.9)
Quan sát phương trình (2.9), chúng ta thấy rằng, khi ma trận hiệp phương sai lỗi đo
lượng R tiến tới 0 thì hệ số khuếch đại K được xác định như sau:
1
0
lim




Hk
k
p

k

Trường hợp khác, khi hiệp phương sai lỗi tiền ước lượng

k
P
tiến tới 0 thì:
0lim
0



k
p
k
k

Khi hiệp phương sai lỗi đo lường R tiến đến 0 thì giá trị z
k
là chính xác hơn, trong khi
giá trị tiên đoán


k
xH
lại kém chính xác. Trường hợp, khi giá trị hiệp phương sai lỗi ước
Bộ lọc Kalman và ứng dụng Page 7

lượng trước


k
P
tiến tới 0, giá trị z
k
là kém chính xác trong khi đó giá trị tiên đoán


k
xH
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
1

1






k
kk
BuxAx

QAAPP
T
kk



1




Cập nhật
thời gian
(tiên đoán)
Cập nhật đo
(sửa sai)
Bộ lọc Kalman và ứng dụng Page 8

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

1
)(

 RHHPHPK
T
k
T
kk

)(





k
kk
k
xHzKxx



kkk
PHKIP )(

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


k

x
quy định trên tất cả
các giá trị đo trước z
k
(Quy tắc Bayes).
k
k
xxE

][

k
T
k
k
k
k
PxxxxE 

]))([(

Trạng thái hậu ước lượng trong phương trình (2.8) phản ánh giá trị trung bình (mômen
bậc 1) của phân bố trạng thái nếu các điều kiện (2.3) được thoả mãn. Hiệp phương sai lỗi
trạng thái hậu ước lượng trong công thức (2.6) phản ánh sự thay đổi của phân bố trạng thái
(mômen bậc 2).
)|(
kk
zxP

),(])))([(],[(

k
k
T
k
k
k
kk
PxNxxxxExEN



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
)()(
)()()(
tCXtY
tButAXtX



(4.1)
Coi tác động nhiễu W(t) và V(t) bằng không khi điều kiện đầu X(t
0
)=X
0
,
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;

Hình 2.2 Sơ đồ cấu trúc của hệ thống liên tục


A
C
B
Y
u(t)
X
Bộ lọc Kalman và ứng dụng Page 9

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(t
0
), đến trạng thái cuối X(t
1
).
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.
 


1
0
t

t
11
TTT
)X(t)G()(tXdtX(t)Q(t)(t)Xu(t)R(t)(t)uJ t
(4.2)
Như vậy, giá trị của hàm J phụ thuộc vào trạng thái ban đầu X(t
0
) tại thời điểm t
0
và
điều khiển u(t) trong khoảng (t
0
, t
1
). 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:
- Biểu thức

1
0
t
t
T
)t(u)t(R)t(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;
- Biểu thức

1
0

t
t
T
)t(X)t(Q)t(X
- Đơn vị chuẩn
)(tX
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
)X(tG(t))(tX
11
T
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  (t
0
, t
1
), chú ý đến hàm tối thiểu J[X(t
0
), u(t), t
0
], cũng như giá
trị của J
*

[X(t
0
), t
0
] 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:

 
   
t),t(u),t(Xf
t
J
t),t(u),t(XFmin
t
J
T*
u
*






khi xác định điều kiện cần thiết để hàm tối thiểu:
Bộ lọc Kalman và ứng dụng Page 10

     

)t(XGd),(u),(XFt,u),t(XJ
1
t
t
00
1
0



khi hạn chế ban đầu dưới dạng phương trình vi phân tuyến tính:
 
00
X)t(X;t),t(u),t(Xf)t(X 


Trong trường hợp của tiêu chuẩn toàn phương (4.2)
F(X,u,t)=u
T
Ru+X
T
QX
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] =
X
T

PX.
Khi đó:
 
XP2PXPXPXX
XX
J
T
TT
T
*












(4.3)
Ngoài ra, trong trường hợp tuyến tính: f(X,u,t)=A(t)X(t)+B(t)u(t) và phù hợp với [4.3].
XPX
t
J
T
*





.
Như vậy, phương trình (4.3) trong khi phân tích áp dụng dạng:

 
 
BuAXPX2QXXRuuminXPX
TTT
u
T



Sau khi biến đổi vế phải của phương trình trên ta nhận được:

   
 














XPAPAPBPBRQX
PXBRuRPXBRu
minXPX
TT1T
T1
T
T1
u
T

(4.4)
Bởi vì ma trận R(t) xác định dương, thì biểu thức (4.4) có cực tiểu, nếu đặt u(t) = -R
-
1
B
T
PX(t). Khi đó phương trình (4.4) dẫn đến dạng:
 
XPAPAPBPBRQXXPX
TT1TT




Phương trình này đúng với mọi X(t), bởi vì:

QPBPBRPAPAP
T1T




(4.5)
Bộ lọc Kalman và ứng dụng Page 11

Đâ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:

 
)t(X)t(P)t(Xt),t(XJ
111
T
11
*

hay
)t(GX)t(X)t(X)t(P)t(X
11
T
111
T

(4.6)
Bởi vì P và G đối xứng, còn X(t
1
) bất kỳ thì: P(t
1

) = 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:

)(),()()()(
1*
tXttPtBtRtu
T

(4.8)
Còn giá trị tối ưu của tiêu chuẩn chất lượng:
)(),()(
010
*
tXttPtXJ
T

(4.9)
trong đó P(t,t
1
) 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 u

T
Ru 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 u
T
Ru đư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:
)()()(
*
tXtKtu 
với
)()()()(
1
tPtBtRtK
T

. Luật điều chỉnh (4.8) được phát biểu
như sau:
Bộ lọc Kalman và ứng dụng Page 12

- Đố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:

const)T,t(Plim)T,t(PlimP
TT



còn
0P

và tất cả các ma trận bên vế phải của phương trình Riccati không phụ thuộc vào
thời gian. Trong trường hợp này luật điều chỉnh

)t(KX)t(XPBR)t(u

1*


(4.10)
với
constPBRK
1



Thêm vào đó
P
xác định bằng nghiệm của phương trình đại số phi tuyến

0QPBBRPPAAP
T1T


;
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
Bộ lọc Kalman và ứng dụng Page 13

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:

)T,t(Plim)tT,0(Plim)T,t(PlimP
TTT 

(4.11)
- Điều kiện ổn định: khi chuyển luật điều chỉnh tối ưu (4.11) vào phương trình (4.1).
Chúng ta nhận được phương trình động học của hệ kín

 
XPBBRAX
T1


(4.12)
Ký hiệu qua D của ma trận bất kỳ thoả mãn điều kiện DD
T
= Q. Khi đó có thể chỉ ra
tiêu chuẩn ổn định như sau:
Hệ thống cho bởi (4.12) ổn định tiệm cận, nếu cặp [A,D] quan sát được hoàn toàn,
đồng thời biểu thức
XPX
T
là hàm Liapunốp.
Đây là tiêu chuẩn đảm bảo tính ổn định của hệ thống kín tối ưu không phụ thuộc sự ổn
định hay không ổn định của đối tượng điều chỉnh.
2.6 Bộ lọc Kalman mở rộng
Ở các phần trên đã mô tả bộ lọc Kalman giải quyết các vấn đề trong một hệ thống
tuyến tính thời gian rời rạc, được cho bởi các phương trình sai phân xác định. Điều gì sẽ xảy

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à:

Bộ lọc Kalman và ứng dụng Page 14

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:
Bộ lọc Kalman và ứng dụng Page 15



 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à:

Và sai số đo đạc:


Chú ý rằng chúng ta không có x
k
ở phương trình trên vì đây chính là giá trị chúng ta
đang cố gắng để ước lượng. Mặc khác, chúng ta chỉ có thể sử dụng z
k
ở phương trình dưới,
vì đây là giá trị thực tế đo được, dùng để ước lượng x
k
. Chúng ta có thể viết lại 2 phương
trình trên như sau:

Trong đó và là những biến ngẫu nhiên độc lập, có hàm trọng bằng 0 và ma trận
hiệp phương sai lần lượt là và .
Bây giờ chúng ta coi như 2 phương trình trên là gần như phương trình sai phân tuyến
tính, và có thể sử dụng như bộ lọc Kalman rời rạc. Điều này sẽ giúp chúng ta ước lượng
được sai lệch giữa giá trị dự đoán và giá trị xấp xỉ của x
k
, gọi 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:
Bộ lọc Kalman và ứng dụng Page 16


2.7 Mô hình hệ thống và thông số
Từ những biến đổi trên, ta có thể rút ra được những phương trình đặc trưng cho các
quá trình của bộ lọc Kalman mở rộng:
 Phương trình cập nhật trạng thái theo thời gian:


 Phương trình cập nhật thông số:

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

2.8 Các bƣớc thiết kế bộ lọc Kalman
Bộ lọc Kalman và ứng dụng Page 17

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:
 
0
là trạng thái ban đầu
 
0
là 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

Bộ lọc Kalman và ứng dụng Page 18

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
1
)(

 RHHPHPK
T
k
T
kk

)(





k
kk
k
xHzKxx

Tiên đoán
1
1







k
kk
BuxAx

QAAPP
T
kk



1


Cập nhật Hiệp phương sai


kkk
PHKIP )(

Khởi tạo
Bộ lọc Kalman và ứng dụng Page 19

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

3.2 Dự đoán
3.2.1 Bộ lọc Kalman tuyến tính
1
1







k
kk
BuxAx

QAAPP
T
kk



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
Bộ lọc Kalman và ứng dụng Page 20

các giá trị ước lượng trước đó và giá trị đo tại thời điểm đang xét để tính toán các thông số
dự đoán chính xác hơn do giá trị khởi tạo có thể không chính xác ngay được.
3.2.2 Bộ lọc Kalman phi tuyến

Giá trị dự đoán đối với quá trình phi tuyến là hàm phụ thuộc vào giá trị ước lượng ở
trạng thái trước đó 
1
, vector điều khiển quá trình 

, với nhiễu hệ thống được xem là
nhiễu trắng theo phân bố Gauss có hàm trọng bằng 0. Phương trình thứ hai là ma trận hiệp
biến sai số ước lượng quá trình tiên nghiệm, với 

và 

là ma trận Jacobian tại thời điểm
k, 

là ma trận hiệp biến nhiễu quá trình tại thời điểm k.
3.3 Hiệu chỉnh
3.3.1 Bộ lọc Kalman tuyến tính

Sau khi ước lượng trạng thái ban đầu, bộ lọc tiếp tục hiệu chỉnh các thông số như: hệ
số khuếch đại của bộ lọc dựa vào giá trị ước đoán và giá trị sai lệch (thông số hiệp phương
sai) và ma trận đo lường H tuy nhiên trong trường hợp ma trận H là hằng số. Hệ số khuếch
đại của bộ lọc được tính toán theo công thức sau:
1
)(

 RHHPHPK
T
k
T
kk


3.3.2 Bộ lọc Kalman phi tuyến

Giá trị ước lượng tiền nghiệm được hiệu chỉnh từ ước lượng tiên nghiệm , độ lợi
của bộ lọc , giá trị đo đạc được tại thời điểm k là và hàm phi tuyến của h biểu diễn
quan hệ giữa giá trị đo được từ cảm biến và ước lượng tiên nghiệm với nhiễu đo đạc được
xem là nhiễu trắng tuân theo phân bố Gauss có hàm trọng bằng 0.
3.4 Giá trị trả về
Giá trị trả về đó chính là thông số ước lượng được (vị trí, vận tốc,…) sau khi tiên đoán,
cứ như vậy cho đến khi ta có được bình phương giá trị sai lệch là bé nhất.
Giá trị trả về là ước lượng vị trí của robot (tọa độ x, y, và góc phi), và cập nhật ma trận
hiệp phương sai số giữa vị trí ước lượng tiền nghiệm và vị trí thực tế của robot tại mỗi thời
điểm k:


Bộ lọc Kalman và ứng dụng Page 21

3.5 Chƣơng trình mô phỏng từng ứng dụng
3.5.1 Hệ thống bám đuổi Radar (Radar Tracking System)
3.5.1.1 Sơ đồ khối hệ thống bám đuổi Radar


Hình 3.3: Sơ đồ hệ thống bám đuổi Radar

Hình 3.4: Sơ đồ khối bộ lọc Kalman
Như đã nói ở phần đầu, khối bộ lọc Kalman sẽ ước lượng vị trí và vận tốc của máy
bay. Khối này sẽ lấy mẫu các phép đo thực tế bị nhiễu tác động, chuyển thành các tọa độ
cực và đưa vào ngõ vào của khối Kalman Filter (ngõ vào Z). Ngõ ra của khối Kalman Filter
sẽ có 2 giá trị. Giá trị đầu là đánh giá vị trị thực, giá trị này sẽ được chuyển đổi thành tọa độ
cực để có thể so sánh với các giá trị đo thực tế ở trên để có thể lấy được sai lệch, sự khác
nhau giữa giá trị ước lượng và thực tế. Khối Kalman Filter sẽ làm trơn dữ liệu vị trí đã đo
được để đưa ra ước lượng vị trí thực của nó.
Bộ lọc Kalman và ứng dụng Page 22

Ngõ ra thứ hai từ khối bộ lọc Kalman sẽ ước lượng trạng thái của máy bay. Trong
trường hợp này, trạng thái được mô tả bằng 1 vector có 4 giá trị gồm có vị trí, vận tốc, các
tọa độ x,y của nó.
3.5.1.2 Kết quả mô phỏng:
* Các thông số khởi tạo:
- Vector trạng thái ước lượng: X= [0.001; 0.01; 0.001; 400]
- Vector hiệp phương sai lỗi ước lượng: P= zeros(4)
- Ma trận chuyển trạng thái:A= [1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1]
- Hiệp phương sai nhiễu quá trình:Q= diag([0 0.005 0 0.005])
- Ma trận đo lường H= [1 0 0 0; 0 0 1 0]
- Hiệp phương sai nhiễu đo lường: R= 300*eye(2)

Hình 3.5: Quỹ đạo thực và quỹ đạo ước lượng

Bộ lọc Kalman và ứng dụng Page 23


Hình 3.6: Lỗi ước lượng trong bộ lọc Kalman

Hình 3.7: Vị trí mục tiêu theo trục X (Bắc-Nam)

Hình 3.8: Vị trí mục tiêu theo trục Y (Đông-Tây)
Bộ lọc Kalman và ứng dụng Page 24

3.5.2 Hệ thống định vị Robot (Robot Localization)
3.5.2.1 Thiết lập giá trị ban đầu
Robot được điều khiển theo quỹ đạo cho trước, với cảm biến vị trí được đo đạc từ hệ
thống GPS để hiệu chỉnh vị trí robot.
Vị trí robot được xác định ban đầu bao gồm:
 vị trí robot (tọa độ x, tọa độ y và góc lệch φ): 
0
= [200; 200; 2];
 ma trận hiệp phương sai lỗi ước lượng 
0
= [1,0,0; 0,1,0; 0,0,1.0e-06];
Nhiễu quá trình được khởi tạo:
 w = [0; 0; 0]; zero mean
 
0
= [0.5, 0, 0; 0,0.5, 0; 0,0,0.5];
Hàm điều khiển quá trình:
 Vector điều khiển u = [delta D, delta phi] = [10,0];
Ma trận chuyển trạng thái A là ma trận Jacobian của f theo x:
A(1,1) = 1;

A(1,2) = 0;
A(1,3) = -delta_D*sin(φ +(delta_phi/2));
A(2,1) = 0;
A(2,2) = 1;
A(2,3) = delta_D*cos(φ + (delta_phi/2));
A(3,1) = 0;
A(3,2) = 0;
A(3,3) = 1;
Ma trận điều khiển B là ma trận Jacobian của f theo x:
B(1,1) = cos( s.value(3,1) + ctrl.value(2,1)/2 ) ;
B(1,2) = ctrl.value(1,1)*sin(s.value(3,1)+ctrl.value(2,1)/2 )*(1/2) ;
B(2,1) = sin( s.value(3,1) + ctrl.value(2,1)/2 ) ;
B(2,2) = ctrl.value(1,1)*-cos( s.value(3,1)+ctrl.value(2,1)/2 )*(1/2) ;
B(3,1) = 0;
Bộ lọc Kalman và ứng dụng Page 25

B(3,2) = 1;
Ma trận hiệp phương nhiễu quá trình Q:
Q = [1,0,0;0,1,0;0,0,1];
Ma trận đo lường H:
H = [1,0,0;0,1,0;0,0,1];
Ma trận hiệp phương nhiễu đo lường R:
R = [1,0,0;0,1,0;0,0,1];
3.5.2.2 Kết quả mô phỏng



Hình 3.9: Quỹ đạo yêu cầu

Hình 3.10: Hành trình của robot được ước lượng bởi bộ lọc