TRNG I HC BCH KHOA H NI

VIN CNG NGH THNG TIN V TRUYN THNG

----------

BI TP LN

X L TN HIU NGU NHIN

TI:
TM HIU L THUYT V CC NG DNG CA B LC
KALMAN

Nhm sinh vin thc hin:

Bi nh Cng
Nguyn Khnh Hng

20080355
20081279

Gio vin hng dn: PGS.TS Nguyn Linh Giang

H NI 8-2012
L thuyt v cc ng dng ca b lc KalmanPage 1

LI GII THIU..........................................................................................................................3
I.

L THUYT B LC KALMAN.......................................................................................4
1.

L thuyt v c lng............................................................................................ 4
1.1.

Khi nim...................................................................................................... 4

1.2.

nh gi cht lng.......................................................................................... 4

1.3.

K vng (Expectation)....................................................................................... 5

1.4.

Phng sai (Variance)....................................................................................... 6

1.5.

lch chun................................................................................................. 7

1.6.

Hip phng sai (Covariance).............................................................................7

1.7.

Ma trn hip phng sai..................................................................................... 8

1.8.

Phn phi chun (phn phi Gaussian)...................................................................8

1.9.

c lng ca trung bnh v phng sai..............................................................10

1.10.
2.

Phng php bnh phng ti thiu..................................................................11

B lc Kalman..................................................................................................... 12
2.1.

Gii thiu chung v b lc Kalman.....................................................................12

2.2.

M hnh ton hc........................................................................................... 15

2.2.1.

H thng v m hnh quan st.....................................................................15

2.2.2.

Gi thit................................................................................................ 15

2.2.3.

Ngun gc............................................................................................. 16

2.2.4.

iu kin khng chch..............................................................................17

2.2.5.

Hip phng sai sai s.............................................................................. 19

2.2.6.

li Kalman........................................................................................ 20

2.2.7.

Tm tt cc phng trnh ca b lc Kalman...................................................21

II.

NG DNG CA B LC KALMAN.........................................................................24

III.

CI T TH NGHIM................................................................................................26

1.

Tao nhiu Gaussian............................................................................................... 26

2.

Ci t b lc Kalman........................................................................................... 27

IV.

2.1.

M phng hot ng ca b lc Kalman...............................................................27

2.2.

M phng hot ng ca b lc Kalman m rng...................................................31

KT LUN........................................................................................................................35

TI LIU THAM KHO...........................................................................................................36

L thuyt v cc ng dng ca b lc KalmanPage 2

LI GII THIU

Ngy nay, nn cng ngh th gii ang pht trin nhanh chng v hng lot cc
gii php cng ngh ra i mi nm. Theo , cc sinh vin ngnh cng ngh ngoi vic
tip thu cc kin thc ging ng cn phi tm hiu nghin cu thm cc cng ngh
tin tin trn th gii c th p ng c yu cu cao ca th trng lao ng. Trong
nhng nm gn y cc loi cm bin, thit b o lng c s dng rng ri trong dn
dng cng nh trong cng nghip. Th nhng nhiu loi thit b li rt nhy vi nhiu,
vn lm sao loi nhiu ra khi tn hiu l mt vn thc s khng n gin.
Vi nhng u im vt tri, tim nng ng dng ca thut ton Kalman vo
thc t trong vic p dng lc nhiu trong tn hiu l rt kh quan, v vy vic nghin
cu nm r v tin ti lm ch phng php ny l rt cn thit v b ch. Ngoi ra
vi mong mun p dng v lp trnh thut ton Kalman vo thc t nn nhm chng em
chn ti: TM HIU L THUYT V CC NG DNG CA B LC
KALMAN.

L thuyt v cc ng dng ca b lc KalmanPage 3

I.

L THUYT B LC KALMAN
Vo nm 1960, R.E Kalman cng b bi bo ni ting v mt gii php truy

hi gii quyt mt bi ton lc thng tin ri rc truyn tnh (discrete data linear
filtering). Tn y ca bi bo l A New Approach to Linear Filtering and Prediction
Problems. T n nay cng vi s pht trin ca tnh ton k thut s, b lc Kalman
tr thnh ch nghin cu si ni v c ng dng trong nhiu ngnh k thut
cng ngh khc nhau: trong t ng ha, trong nh v cng nh trong vin thng v
trong nhiu lnh vc khc.
Mt cch khi qut, b lc Kalman l mt tp hp cc phng trnh ton hc m
t mt phng php tnh ton truy hi hiu qa cho php c on trng thi ca mt qu
trnh sao cho trung bnh phng sai ca l nh nht. B lc Kalman rt hiu qu trong
vic c on cc trng thi trong qu kh, hin ti v tng lai thm ch ngay c khi tnh
chnh xc ca h thng m phng khng c khng nh.

1. L thuyt v c lng
1.1.

Khi nim
Trong thng k, mt c lng l mt gi tr c tnh ton t mt mu th v

ngi ta hy vng l gi tr tiu biu cho gi tr cn xc nh trong tp hp. Ngi ta

lun tm mt c lng sao cho l c lng khng chch, hi t, hiu qu v
vng(robust)..
1.2. nh gi cht lng
Mt c lng l mt gi tr x c tnh ton trn mt mu c ly mt cch
ngu nhin, do gi tr ca x l mt bin ngu nhin vi k vng E(x) v phng sai
V(x). Ngha l gi tr x c th dao ng ty theo mu th, n c t c hi c th bng
ng chnh xc gi tr X m n ang c lng. Mc ch y l ta mun c th kim
sot s sai lch gi tr x v gi tr X.
Mt bin ngu nhin lun dao ng xung quanh gi tr k vng ca n. Ta mun l
k vng ca x phi bng X. Khi ta ni c lng l khng chch. Trung bnh tch ly
trong v d v chiu cao trung bnh ca tr 10 tui mt c lng ng, trong khi c
lng v tng s c trong h c tnh nh trong v d l mt c lng khng ng,
L thuyt v cc ng dng ca b lc KalmanPage 4

l c lng tha: trung bnh tng s c c lng c lun ln hn tng s c c thc

trong h.
Ta cng mun l khi mu th cng rng, th sai lch gia x v X cng nh. Khi
ta ni c lng l hi t. nh ngha theo ngn ng ton hc l nh sau:
lim p ( x n X )=0

(xn) hi t nu

vi mi s thc dng (xc sut sai lch vi

gi tr thc cn c lng ln hn tin v 0 khi kch c ca mu th cng ln).

Bin ngu nhin dao ng quanh gi tr k vng ca n. Nu phng sai V(x) cng
b, th s dao ng cng yu. V vy ta mun phng sai ca c lng l nh nht c
th. Khi ta ni c lng l hiu qu.
Cui cng, trong qu trnh iu tra, c th xut hin mt gi tr bt thng (v d
c tr 10 tui nhng cao 1,80 m). Ta mun gi tr bt thng ny khng nh hng qu
nhiu n gi tr c lng. Khi ta ni c lng l vng. C th thy trung bnh tch
ly trong v d v chiu cao trung bnh tr 10 tui khng phi l mt c lng vng.
1.3.

K vng (Expectation)
nh ngha: Gi s

x1 , x2, , xn

l i lng ngu nhin ri rc c th nhn cc gi tr

vi cc xc sut tng ng

P 1 , P2 , , P n

Khi k vng ca X, k hiu l E ( X ) hay c xc nh bi cng thc

n

E ( X )= xi Pi (1.1)
i=1

Nu X

l i lng ngu nhin lin tc c hm mt xc sut l f ( x ) th k

vng ca X l:
+

E ( X )= xf ( x ) dx (1.2)

L thuyt v cc ng dng ca b lc KalmanPage 5

Tnh cht
E ( C ) =C

i.

E ( C . X )=C . E ( X ) , vi C

ii.

l hng s.

E ( X +Y )=E ( X ) + E ( Y )

iii.
iv.

Nu X v Y l hai i lng ngu nhin c lp th:

E ( X .Y )=E ( X ) . E ( Y )

ngha: K vng ca mt i lng ngu nhin chnh l gi tr trung bnh (theo xc

sut) ca i lng ngu nhin . N l im trung tm ca phn phi m cc gi tr c
th ca X s tp trung quanh .
1.4. Phng sai (Variance)
nh ngha: Phng sai (trung bnh bnh phng lch) ca i lng ngu
nhin X, k hiu Var ( X ) hay V ( X ) c xc nh bi cng thc:
Var ( X )=E [ ( X)2 ] (1.3)

Nu X l i lng ngu nhin ri rc c th nhn cc gi tr

xc xc sut tng ng l
n

P 1 , P2 , , P n

x1 , x2, , xn

th:

Var ( X )= [ x i ] Pi (1.4)
i=1

Nu X l i lng ngu nhin lin tc c hm mt xc sut l f ( x ) th:

+

Var ( X )= [ x ] f ( x ) dx( 1.5)

Trong thc t ta thng tnh phng sai bng cng thc:

2

Var ( X )=E [ X 2 ][ E ( X ) ] (1.6)

Tnh cht:
i.

Var ( C )=0

L thuyt v cc ng dng ca b lc KalmanPage 6

vi

Var ( C . X )=C 2 . Var ( X ) ;

ii.

iii. Nu X, Y l 2 bin ngu nhin c lp th:

Var ( X +Y )=Var ( X Y )=Var ( X ) +Var ( Y ) (1.7)

ngha:

l lch khi gi tr trung bnh. Do phng sai

Var ( X )

gi l trung bnh bnh phng lch. Nn phng sai phn nh mc phn tn ca

cc gi tr ca i lng ngu nhin quanh gi tr trung bnh hay k vng. i lng ngu
nhin c phng sai cng ln th gi tr cng phn tn v ngc li.
1.5.

lch chun
nh ngha: lch chun ca i lng ngu nhin X, k hiu

( X)

xc nh bi cng thc:
( X ) =Var ( X ) (1.8)

1.6.

Hip phng sai (Covariance)

Cho 2 bin ngu nhin X v Y, ta c nh ngha hip phng sai ca X v Y, k

hiu Cov ( X , Y ) :
Cov ( X , Y )=E [ ( X x )( Y y ) ] (1.9)

trong

x , y

ln lt l k vng ca X, Y.

Mt cng thc tng ng ca hip phng sai:

Cov ( X , Y )=E [ XY ] x y (1.10)

ngha ca hip phng sai l s bin thin cng nhau ca 2 bin ngu nhin:
Nu 2 bin c xu hng thay i cng nhau (ngha l, khi mt bin c gi tr cao hn k
vng th bin kia cng c xu hng cao hn k vng), th hip phng sai ca hai bin
ny c gi tr dng. Mt khc, nu mt bin nm trn gi tr k vng cn bin kia c xu
hng nm di gi tr k vng, th hip phng sai ca hai bin c gi tr m.
L thuyt v cc ng dng ca b lc KalmanPage 7

Nu 2 bin ngu nhin l c lp th

Cov ( X , Y )=0

tuy nhin iu ngc li

khng ng. Cc bin ngu nhin m c hip phng sai bng 0 c gi l khng tng
quan (uncorrelated), chng c th c lp nhau hoc khng.
Nh vy nu X, Y c lp ta c E [ XY ] = x y .
Tnh cht

1.7.

Cov ( X , X )=Var ( X )

Cov ( X , Y )=Cov ( Y , X )

Cov ( aX , bY )=ab Cov ( X ,Y )

Cov ( X 1 + X 2 , Y 1 +Y 2 )=Cov ( X 1 ,Y 1 ) +Cov ( X 2 ,Y 1 ) +Cov ( X 1 ,Y 2) + Cov ( X 2 , Y 2 )

Var ( X +Y )=Var ( X ) +Var ( Y ) +2Cov ( X , Y )

Ma trn hip phng sai

Nh chng ta va trnh by, hip phng sai l i lng tnh ton s tng quan

gia 2 bin ngu nhin.

Vy gi s chng ta c mt vector bin ngu nhin c 3 phn t

X1 , X2 , X3

. Nu

ta mun tnh ton s tng quan gia tt c cc cp bin ngu nhin th ta phi tnh tt c
3 hip phng sai

Cov ( X 1 , X 2 ) , Cov ( X 1 , X 3 ) , Cov( X 2 , X 3 )

Mt cch tng qut, ma trn hip phng sai ra i cho php ta tnh tt c
cc Cov gia 2 bin ngu nhin trong mt vector bin ngu nhin.
Cho mt vector bin ngu nhin X cha n bin ngu nhin, ma trn hip phng
sai ca X, k hiu l , c nh ngha l:

Cov ( X 1 , X 1 ) Cov ( X 1 , X 2 )

Cov ( X 1 , X n )
Cov ( X 2 , X n )
Cov ( X 2 , X 1 ) Cov ( X 2 , X 2 )

Cov ( X n , X 1 ) Cov ( X n , X 2 ) Cov ( X n , X n )

L thuyt v cc ng dng ca b lc KalmanPage 8

Vi

[]

X1
X=
Xn

Quan st trn ng cho ca ma trn hip phng sai (i=j) ta thy ti l cc

phng sai, v
1.8.

Cov ( X i , X i ) =Var ( X i )

Phn phi chun (phn phi Gaussian)

Trong thc t, ngi ta thng s dng phn phi xc sut c tn l phn phi

chun (normal distribution) hay phn phi Gaussian.

Mt bin ngu nhin X c gi l c phn phi Gaussian khi n c hm mt
X N (,)

l hm Gaussian, k hiu l

gi l X c phn phi chun vi tham s

, . Khi hm mt ca X l:
2

f ( x ; , )=

1
e
2

( x )
2
2

Vi phn phi xc sut nh trn, ngi ta tnh c

lch chun ca X.
Di y l th ca mt s phn phi chun.

L thuyt v cc ng dng ca b lc KalmanPage 9

ln lt l k vng v

Quan st th ta thy phn phi chun c dng chung. Gi tr k vng ca X l

X =

l trc i xng. lch chun

2
(hay phng sai ) cng ln th th

cng bt, ngha l cc gi tr cng phn tn ra xa k vng.

Trong thc t, cc loi nhiu trong cc h thng o lng c th c m phng
mt cch chnh xc bng nhiu trng cng. Hay ni cch khc tp m trng Gaussian l
loi nhiu ph bin nht trong h thng o lng. Loi nhiu ny c mt ph cng
sut ng u trn min tn s v bin tun theo phn b Gaussian. Theo phng thc
tc ng th nhiu Gaussian l nhiu cng. Vy cc h thng o lng ph bin chu tc
ng ca nhiu Gaussian trng cng (AWGN).

Hnh 1.1: th ca mt s phn phi chun

L thuyt v cc ng dng ca b lc KalmanPage 10

Hnh 1.2: Nhiu Gaussian

1.9.

c lng ca trung bnh v phng sai

Ta chn ngu nhin n c th trong mt dn s gm N c th. Ta quan tm n c

trng nh lng Y ca dn s vi trung bnh

v phng sai V(Y). Trong mu ,

c trng Y c trung bnh v phng sai o c ln lt l

Lu l cc gi tr

1
2
( y i y ) .

n i=1

v 2 thay i ty theo mu th, do chng l cc bin ngu

nhin vi trung bnh v phng sai ring khc nhau.

c lng trung bnh ca Y:
n

Thng thng trung bnh ca Y, tc l

c c lng bi:

y =

1
y
n i =1 i ,

cn c gi l trung bnh tch ly (hay trung bnh cng). Ta chng minh c y l

c lng khng chch (unbiased), ngha l E ( y )=Y
c lng phng sai ca Y:
L thuyt v cc ng dng ca b lc KalmanPage 11

2 l mt c lng ca V(Y), nhng l c lng khng ng, ta chng minh

c k vng ca 2 lun nh hn V(Y), tc c lng l thiu.
Cc c lng ng ca V(Y) l:
n
2 (1.11) trong trng hp ly mu c hon li
n1
N
n 2
(1.12) trong trng hp ly mu khng hon li
N 1 n1

Trong trng hp mu ln, php tnh c hon li v php tnh khng hon li l
nh nhau, v

N
N 1

ca V(Y) l:

1
2
s=
( y i y )

n1 i=1
2

xp x bng 1. V vy trong trng hp tng qut c lng ng

c gi l phng sai tch ly ca Y.

1.10. Phng php bnh phng ti thiu

Trong ton hc, phng php bnh phng ti thiu, cn gi l bnh phng nh
nht hay bnh phng trung bnh ti thiu, l mt phng php ti u ha la chn
mt ng khp nht cho mt di d liu ng vi cc tr ca tng cc sai s thng k
(error) gia ng khp v d liu.
Phng php ny gi nh cc sai s (error) ca php o c d liu phn phi
ngu nhin. nh l Gauss-Markov chng minh rng kt qu thu c t phng php
bnh phng ti thiu khng thin v v sai s ca vic o c d liu khng nht thit
phi tun theo, v d, phn b Gauss. Mt phng php m rng t phng php ny l
bnh phng ti thiu c trng s.
Phng php bnh phng ti thiu thng c dng trong khp ng cong.
Nhiu bi ton ti u ha cng c quy v vic tm cc tr ca dng bnh phng, v d
nh tm cc tiu ca nng lng hay cc i ca entropy.
Gi s d liu gm cc im (x i, yi) vi i = 1, 2, ..., n. Chng ta cn tm mt hm
s f tha mn:
L thuyt v cc ng dng ca b lc KalmanPage 12

f ( x i ) y i (1.13)

Gi s hm f c th thay i hnh dng, ph thuc vo mt s tham s, pj vi j = 1,

2, ..., m.
p
f ( j , x )(1.14)
f (x)

Ni dung ca phng php l tm gi tr ca cc tham s pj sao cho biu thc sau

t cc tiu:
n

= ( y if (x i))2 (1.15)
2

i=1

Ni dung ny gii thch ti sao tn ca phng php l bnh phng ti thiu.

i khi thay v tm gi tr nh nht ca tng bnh phng, ngi ta c th tm gi
tr nh nht ca bnh phng trung bnh:
n

1
2
= ( y if ( x i)) (1.16)
n i=1
2

iu ny dn n tn gi bnh phng trung bnh ti thiu.

Trong hi quy tuyn tnh, ngi ta thay biu thc
f ( xi ) yi

bng
f ( x i )= y i+ i (1.17)

vi h s nhiu l bin ngu nhin c gi tr k vng bng 0.

Trong biu thc ca hi quy tuyn tnh x c o chnh xc, ch c y chu nhiu lon .
Thm na, hm f tuyn tnh vi cc tham s pj. Nu f khng tuyn tnh vi cc tham s,
ta c hi quy phi tuyn, mt bi ton phc tp hn nhiu hi quy tuyn tnh.

2. B lc Kalman
2.1.

Gii thiu chung v b lc Kalman

L thuyt v cc ng dng ca b lc KalmanPage 13

c xut t nm 1960 bi gio s Kalman thu thp v kt hp linh ng

cc thng tin t cm bin thnh phn. Mt khi phng trnh nh hng v mu thng k
nhiu trn mi cm bin c bit v xc nh, b lc Kalman s cho c lng gi tr
ti u (chnh xc do c loi sai s, nhiu) nh l ang s dng mt tn hiu tinh
khit v c phn b khng i. Trong h thng ny, tn hiu cm bin vo b lc gm
hai tn hiu: t cm bin gc (inclinometer) v cm bin vn tc gc (gyro). Tn hiu u
ra ca b lc l tn hiu ca inclinometer v gyro c loi nhiu nh hai ngun tn
hiu h tr v x l ln nhau trong b lc, thng qua quan h (vn tc gc = o hm/vi
phn ca gi tr gc.

L thuyt v cc ng dng ca b lc KalmanPage 14

B lc Kalman n gin l thut ton x l d liu hi quy ti u. C nhiu cch

xc nh ti u, ph thuc tiu chun la chn trnh thng s nh gi. N cho thy rng
b lc Kalman ti u i vi chi tit c th trong bt k tiu chun c ngha no. Mt
kha cnh ca s ti u ny l b lc Kalman hp nht tt c thng tin c cung cp ti
n. N x l tt c gi tr sn c, ngoi tr sai s, c lng gi tr hin thi ca
nhng gi tr quan tm, vi cch s dng hiu bit ng hc thit b gi tr v h thng,

m t s liu thng k ca h thng nhiu, gm nhiu n, nhiu o v s khng chc chn

trong m hnh ng hc, v nhng thng tin bt k v iu kin ban u ca gi tr quan
tm.

L thuyt v cc ng dng ca b lc KalmanPage 15

Hnh 1.3: M hnh o lng c lng ca b lc Kalman

Hnh 1.4: Tn hiu thu trc v sau khi lc qua Kalman

Hnh 1.3 trn m hnh ha hot ng ca mch lc Kalman. Chng ta c tn hiu

o c, chng ta c m hnh ca tn hiu o c (i hi tuyn tnh) v sau l p
dng vo trong h thng phng trnh ca mch lc c lng trng thi quan tm.
Thc ra tn hiu o l khng kh, phng trnh c sn, ci chung ta cn chnh l m
hnh ho h thng. c th ng dng mt cch hiu qu mch lc Kalman th chng ta
phi m hnh ha c mt cch tuyn tnh s thay i ca trng thi cn c lng hoc
d on.

L thuyt v cc ng dng ca b lc KalmanPage 16

2.2. M hnh ton hc

2.2.1. H thng v m hnh quan st
Chng ta gi s rng c th m hnh ha bi phng trnh chuyn trng thi
x k+1=F k x k +G k uk +w k (2.1)

Trong
wk

xk

uk

l trng thi ti thi im k,

l vector iu khin u vo,

l h thng cng hay nhiu qu trnh thng l nhiu Gaussian trng cng

(AWGN) ,

Gk

l ma trn chuyn i u vo v

Fk

l ma trn chuyn trng thi.

Ngoi ra chng ta gi s rng, kh nng quan st trng thi c thc hin thng
qua mt h thng o lng c th c biu din bi mt phng trnh tuyn tnh nh
sau
z k =H k x k + v k (2.2)

Trong
xk

zk

l thng tin quan st hay o lng thc hin ti thi im

Hnh 2.1: M hnh
khng gian trng thi
k , H k l ma trn quan st v

l trng thi ti thi im

trong qu trnh o lng.

2.2.2. Gi thit
Chng ta gi thit nh sau
L thuyt v cc ng dng ca b lc KalmanPage 17

vk

k ,

l nhiu cng

wk

Nhiu qu trnh v nhiu o lng

vk

l khng tng quan, l

nhiu Gaussian trng cng (AWGN) c gi tr trung bnh bng khng v ma

trn hip phng sai bit.
Khi ,
E [ w k wTl ]=
E [ v k vTl ]=

Q k =l (2.3)
{0 otherwise
,
k

R k =l (2.4)
{0 otherwise
,
k

E [ w k v Tl ] =0 for all k ,l(2.5)

Qk

Trong

Rk

l cc ma trn i xng na xc nh dng.

Trng thi khi to h thng

x0

l mt vector ngu nhin khng tng

quan vi c h thng v nhiu o lng.

Trng thi khi to h thng c gi tr trung bnh v ma trn hip phng
sai bit.
^x 00=E [ x 0 ] v P 00=E [( x^ 00x 0 )( ^x 00x 0)T ] (2.6)

a ra nhng gi nh trn vi mc ch xc nh, a ra tp gi tr quan st

z 1 , , z k+1

thi

x k+1

, b lc c lng thi im k +1 to ra mt c lng ti u ca trng

m chng ta k hiu bi

^x k+1

, ti thiu ha k vng ca hm tn tht bnh

phng li.

E x k +1 ^x k+1 =E [ (x k+1 ^x k+1 )T ( x k +1 ^x k+1 ) ] (2.7)

2.2.3. Ngun gc
K hiu c lng d on ca trng thi
k ,

z1 , , zk

^x k+1k

x k+1

da trn quan st thi im

. c gi l mt bc trc d on hay n gin l d

on. By gi, gii php ti thiu ha phng trnh (2.7) l k vng ca trng thi
thi im k +1 c c nh da trn quan st thi im k . Nh vy,

L thuyt v cc ng dng ca b lc KalmanPage 18

^x k+1k =E [ x k+ 1z1 , , z k ] =E [ x k+1 k ] (2.8)

Khi trng thi d on c cho bi

^x k+1k =E [ x k +1 k ]
E [ Fk x k +Gk u k +w k| k ]
Fk E [ x k k ] +Gk u k + E [ w k k ]
Fk ^x kk +Gk u k ( 2.9 )

Khi s dng trong thc t, nhiu qu trnh c gi tr trung bnh l 0 v

uk

c bit chnh xc.

Hip phng sai c lng d on
trong c lng

^x k+1k

Pk+1k

l trung bnh bnh phng sai s

.
wk

V vy, bng vic s dng cc s kin m

^x kk

l khng tng quan:

Pk+1k =cov ( x k+1^x k+1k )

E [ ( x k+1 ^xk +1k ) ( x k+1^x k+1k ) T k ]
cov ( F k ( x k ^xk k ) + w k ) =F k cov ( x k ^x kk ) F Tk + E [ wk wTk ]
T

Fk Pkk Fk +Qk (2.10)

ang c mt c lng d on
quan st
tm

z k+1

^x k+1k+1

^x k+1k

, gi s rng chng ta ang c mt gi tr

. Lm sao s dng thng tin ny cp nht trng thi d on, tc l

. Chng ta gi s rng c lng l tng trng s tuyn tnh ca d on

v quan st mi v c th c m t bi phng trnh,

L thuyt v cc ng dng ca b lc KalmanPage 19

^x k+1k+1 =K 'k+1 ^x k+1k + K k +1 z k+1 (2.11)

Trong

K 'k+1

K k+1

l nhng ma trn hiu chnh b hay ma trn li

(ca cc kch thc khc nhau). Vn ca chng ta by gi l tm

K 'k+1

K k+1

ti thiu ha iu kin c lng trung bnh bnh phng sai s. Sai s d on c cho
bi
~
x k+1k +1= ^x k+1k+ 1x k+1 (2.12)

2.2.4. iu kin khng chch

b lc khng chch yu cu

E [ ^x k+1k+1 ]=E [ xk +1 ]

. Gi s rng

^x kk

c lng khng chch. Kt hp phng trnh (2.11) v (2.12) v tnh k vng

^x
E[ k +1k +1]=E[ K

'
k+1

^x k+1k + K k +1 H k +1 x k+1 + K k +1 v k+1 ]

K 'k+1 E [ ^x k+1k ] + K k+1 H k+1 E [ x k+1 ] + K k+1 E [ v k +1 ] (2.13)

E [ v k +1 ]=0

, v d on l khng chch:

E [ ^x k+1k ] =E [ F k x^ kk +G k uk ]
Fk E [ ^x kk ]+ Gk u k
E [ x k+1 ] (2.14)

Do kt hp phng trnh (2.13) v (2.14)

K
'

( k +1 + K k +1 H k +1) E [ x k +1 ]
E [ ^x k+1k+1 ] =

V iu kin

^x k+1k+1

khng chch yu cu

L thuyt v cc ng dng ca b lc KalmanPage 20

l mt

'

K k+1 + K k+1 H k+1=I

hay K 'k+1=I K k+1 H k+1 (2.15)

c lng khng chch yu cu

^x k+1k+1 =( I K k+ 1 H k+1 ) ^x k+1k + K k+1 z k +1
^x k+1k + K k +1 [ z k+1H k+1 x^ k +1k ] (2.16)

Trong K c gi l li ca b lc Kalman (Kalman gain).

H k+1 ^x k+1k

c th c hiu nh mt quan st hay o lng d on ^z k+1k

^z k+1k =E [ z k +1 k ]
E [ H k+1 x k+1 +v k +1 k ]
H k+1 ^x k+1k (2.17)

r k +1

c lng ca n

l lch o lng th hin s sai khc gia gi tr o lng

^z k+1k

, c biu din bi

r k +1=z k+1H k +1 x^ k +1k ( 2.18)

E [ r k +1 k ] =E [ z k+1 z^ k +1k k ]
E [ z k+1 k ] z^ k +1k
0(2.19)

Kt hp (2.16) v (2.18) ta c

L thuyt v cc ng dng ca b lc KalmanPage 21

z k+1

^x k+1k+1 ^x k+1k + K k +1 r k +1 (2.20)

S k+1

Hip phng sai lch o lng

c cho bi,

S k+1 =E [ r k+ 1 r Tk +1 ]
E [ ( z k+1 H k+1 ^xk +1k ) ( z k+1H k +1 ^x k+1k ) T ]
S k+1 =H k+1 Pk+1k H Tk+1 + Rk +1 (2.21)

2.2.5. Hip phng sai sai s

Chng ta xc nh hip phng sai sai s d on ca phng trnh (2.10). By
gi chng ta tnh ton hip phng sai sai s iu chnh.
Pk+1k +1=E [ ( ~
x k +1k+1 ) (~x k+1k+1 )T k ]
E [ ( x k+1 ^xk +1k+1 ) ( x k+1 x^ k +1k+1 )

cov ( x k+1 x^ k +1k+1 )

x^

x k+1( k +1k + K k +1 (z k+1H k +1 ^x k+1k ) )

cov
cov ( x k+1 ( ^x k+1k + K k+1 ( H k+1 x k +1+ v k+ 1H k+1 ^x k+1k ) ) )

cov ( ( I K k +1 H k +1 )( x k+1 ^x k+1k ) K k+ 1 v k+1 )

cov ( ( I K k +1 H k +1 )( x k+1 ^x k+1k ) ) +cov ( K k+1 v k+1 )
L thuyt v cc ng dng ca b lc KalmanPage 22

 ( I K k +1 H k +1) cov ( x k+ 1^x k+1k )( I K k+ 1 H k+ 1) T + K k +1 cov ( v k+1 ) K Tk+1

Pk+1k +1=( I K k +1 H k +1 ) P k+1k ( I K k+1 H k+1 )T + K k+1 Rk+1 K Tk +1(2.22)

Ta c th tnh ton

Pk+1k +1

theo cch khc nh sau

Pk+1k +1=cov ( x k+1 x^ k +1k+1 )

cov ( x k+1 x^ k +1k K k+ 1 r k +1 )
cov ( x k+1 x^ k +1k ) cov ( K k +1 r k+1 )
T

Pk+1k K k+1 S k+1 K k+1 (2.23)

Trong ,

Pk+1k =cov ( x k+1^x k+1k )

Rk+ 1=cov ( v k +1 )=E [ v k+1 v Tk +1 ]

V vy hip phng sai ca c lng iu chnh c biu din qua hip

phng sai d on

Pk+1k

, nhiu o lng

Rk+ 1

v ma trn li Kalman

K k+1

2.2.6. li Kalman
Mc tiu ca chng ta l lm sao ti thiu ha trung bnh bnh phng sai s
c lng c iu kin vi li Kalman K .
L=min E [ ( ~
x k +1k+1 )T ( ~
x k+1k +1 ) k ]
K k +1

T
min trace ( E [ ( ~x k+1k+1 ) ( ~
x k +1k+1 ) k ])
K k+ 1

min trace ( Pk+1k +1 ) (2.24)

K k+ 1

Vi bt k ma trn A v ma trn i xng B ta c

L thuyt v cc ng dng ca b lc KalmanPage 23


( trace ( AB A T ) ) =2 AB
A

Kt hp (2.23) v (2.24) v ly vi phn ma trn li v t kt qu bng 0 ta

c
L
=2 ( I K k+1 H k+1 ) P k+1k H Tk+1 +2 K k+1 Rk+1 =0
K k+1

Sp xp li v a ra phng trnh cho ma trn li

1

K k+1 =Pk+1k H Tk+1 [ H k+1 Pk+1k H Tk+1 + R k+1 ] (2.25)

Kt hp vi (2.21) ta c
T

K k+1 =Pk+1k H k+1 S k+1 (2.26)

Cng vi phng trnh 2.16, nh ngha mt c lng ti u tuyn tnh trung

bnh bnh phng sai s.

T (2.26) ta c
K k+1 S k +1 K Tk +1=Pk+1k H Tk+1 K Tk +1(2.27)

Kt hp (2.23) v (2.27)
Pk+1k +1=Pk+ 1k ( I H Tk+1 K Tk+1 )
( Pk +1k ( I H Tk+1 K Tk+1 ) )

( I H Tk +1 K Tk +1 ) ( Pk +1k )T
( I K k +1 H k +1) Pk +1k (2.28)

L thuyt v cc ng dng ca b lc KalmanPage 24

2.2.7. Tm tt cc phng trnh ca b lc Kalman

Trong phn ny chng ta s tm tt cc phng trnh tng qut ca gii thut lc
Kalman. Gii thut bao gm 2 qu trnh: qu trnh c lng v qu trnh iu chnh.
Qu trnh d on
B lc Kalman da vo trng thi c lng iu chnh
xk

c lng trng thi

^x k+1k

^x kk

l c lng d on ca

z k+1

Trng thi d on:

^x k+1k =F k ^x kk +G k uk (2.29)

Hip phng sai c lng d on:

Pk+1k =F k Pkk F Tk +Q k (2.30)

o lng d on:
^z k+1k =H k+1 x^ k +1k (2.31)

Qu trnh iu chnh
lch o lng:
r k +1=z k+1H k +1 x^ k +1k ( 2.32)

Hip phng sai lch:

S k+1 =H k+1 Pk+1k H Tk+1 + Rk +1 (2.33)

li Kalman:
L thuyt v cc ng dng ca b lc KalmanPage 25

- l c lng ca
x k+1

cho php o

K k+1 =Pk+1k H k+1 S k+1 (2.34 )

Trng thi c lng iu chnh:

^x k+1k+1 = ^x k+1k + K k+1 r k +1( 2.35)

Hip phng sai c lng iu chnh:

Pk+1k +1=( I K k +1 H k +1 ) P k+1k (2.36)

Prediction
(1)

Initial

^x k+1k =F k ^x kk +G k uk

T
(2) Pk+1k =F k Pkk F k +Q k

^x kk

and

Update
(3)

S k+1 =H k+1 Pk+1k H Tk+1 + Rk +1

(4)

K k+1 =Pk+1k H Tk+1 S1

k+1

(5)

^x k+1k+1 =^x k+1k + K k+1 ( z k+1 H k+1 ^xk +1k )

(6)

Pk+1k +1=( I K k +1 H k +1 ) P k+1k

Hnh 2.2: Tm tt qu trnh khi to ca Kalman

Cng vi cc iu kin ban u trong c lng v ma trn hip phng sai li

ca n (phng trnh 2.6) nh nha mt gii thut ri rc ha v thi gian v quy
xc nh hip phng sai c lng tuyn tnh ti thiu c gi l b lc Kalman.

L thuyt v cc ng dng ca b lc KalmanPage 26

II.

NG DNG CA B LC KALMAN

Bi v b lc Kalman gii quyt mt s vn c bn l lc nhiu v ti u cho

cc c lng nn n c ng dng rt rng ri. Ngy nay Kalman c ng dng
nhiu trong cc t t li c kh nng thay th con ngi vn hnh xe, mt chng trnh
my tnh c ci sn b lc Kalman s c nhim v iu khin xe. Nhng chic xe ny
thm ch cn c gii thiu l an ton hn xe li bi con ngi trong mt s trng hp.
Mt ng dng khc, c th chng ta khng thch th lm, l cc tn la khng
i khng (air-to-air missile: AAM). l cc tn la dn hng vic bn t mt my
bay tiu dit my bay khc. Tn la dn hng hot ng theo nguyn l pht hin
mc tiu (thng thng bng rada hoc hng ngoi, i khi cng s dng Lazer hoc
quang hc) sau t ng dn n mc tiu nh qu trnh c lng ca Kalman [3].
Ngoi ra b lc Kalman cn c p dng nhiu vo h thng theo di mc tiu di
ng trong mng cm bin khng dy. Do nhiu o lng trn cc cm bin nn kt qu
thu c thng khng chnh xc, c sai s ln so vi thc t. B lc Kalman c p
dng lc nhiu, d on, c lng trng thi ca mc tiu nh v tr, tc v qu
o. Nh c qu trnh d on v iu chnh ca b lc Kalman gp phn quan trng
vo vic qun l trng thi cc cm bin lm gim thiu nng lng tiu th cng nh
tng cht lng theo di v ko di thi gian sng ca mng [4].
Mt s ng dng c lit k t bi vit Kalman Filter trn Wikipedia [5]:

Li t ng my bay (Autopilot)
c lng trng thi sc ca pin (Battery state of charge (SoC) estimation)
Giao din tng tc vi my tnh bng no (Braincomputer interface)
nh v chuyn ng (Dynamic positioning)
Cc ng dng trong kinh t, c bit l kinh t v m, time series, v econometrics
H thng dn ng qun tnh (Inertial guidance system)
Theo di bng radar (Radar tracker)
H thng nh v v tinh (Satellite navigation systems)
D bo thi tit (Weather forecasting)
H thng nh v (Navigation Systems)
M hnh ha 3 chiu (3D-Modelling)

Vit Nam c mt s ng dng nh:

L thuyt v cc ng dng ca b lc KalmanPage 27

 ng dng lc Kalman trong phn tch bin dng nh cao tng do bc x nhit mt
tri.
Ci thin cht lng truyn ng khng ng b bng cu trc tch knh trc tip
s dng kalman filter quan st t thng. [6]
ng dng Kalman Filter cho d bo nhit 2m t sn phm m hnh HRM.
H thng dn ng qun tnh INS/GPS. [7]
S dng b lc Kalman kt hp vi thut ton bm nh Camshift nhm nng cao
cht lng bm trong cc h thng robot t ng tm kim v bm bt mc tiu.[8]

L thuyt v cc ng dng ca b lc KalmanPage 28

III. CI T TH NGHIM
1. Tao nhiu Gaussian
S dng chuyn i Box Muller chuyn t phn phi chun trong khong
[0;1]

2
sang phn phi Gaussian N ( , )

2
Gi s bin ngu nhin x N ( , )

x=+ 2 log (s)cos (2 t)(3.1)

Hay
x=+ 2 log (s)sin(2 t)(3.2)

trong s ,t [0 ;1]
V d to sinh 1000 im tun theo phn phi Gaussian N (0, 1)

Hnh 3.1: To sinh 1000 im

L thuyt v cc ng dng ca b lc KalmanPage 29

N (0, 1)

Trong hnh v trn, th bn tri l hm mt xc sut v phn b ca 1000

im Gaussian. th bn phi th hin s thay i gia phng sai v k vng. T hnh
v ta thy phng sai cng ln th gi tr cng phn tn.
2. Ci t b lc Kalman
2.1. M phng hot ng ca b lc Kalman
Cc thng s ci t
y chng ta s m hnh ha mt mc tiu chuyn ng c vector trng thi ti
thi im k:
x k =[x ( k ) x ( k )

y (k )

y (k )]

Phng trnh chuyn trng thi ca mc tiu:

x k+1=F k x k + wk

Trong :

1 Tk
0
0
1
F k= 0
1
0 0
0 0
0

T k =T k+1T k =1 s

Qk =E [ wk w k T ]

0
0
Tk
1

L thuyt v cc ng dng ca b lc KalmanPage 30

q : l i lng biu din cng ca nhiu qu trnh.

x 00=

[]
1
0
0
0

0
0
z 0
1
0

; P00=Indentity (4, 4)

M hnh o lng:
z k =H k x k + v k

Trong :

H k= 1 0 0 0
0 0 1 0

v k N (0, 1)

Rk =E [ v k v k ]=

=5 : l i lng biu din cng ca nhiu o lng.

[ ]
1 0
0 1

Kt qu ci t
Ngn ng s dng C#
Input: Qu o chuyn ng ca mc tiu c v bng con tr chut c

thm nhiu Gaussian.

- Output: Qu o c c lng.
Giao din

L thuyt v cc ng dng ca b lc KalmanPage 31

 S dng

Dng con tr chut v mt qu o chuyn ng bt k ca mc tiu v c

biu din bi ng mu en.
Khi nhn Draw chng trnh s to d liu o lng th hin qu trnh o
lng v tr ca mc tiu c th hin bi ng mu .
B lc Kalman s cho kt qu l ng mu xanh chnh l d liu c c
lng ti u.
Chng ta c th thay i cc thng s t kt qu ti u.
Kt qu

Qu o thc ca mc tiu:

L thuyt v cc ng dng ca b lc KalmanPage 32

L thuyt v cc ng dng ca b lc KalmanPage 33

 Qu o o lng

Qu o sau khi qua lc Kalman

2.2.

M phng hot ng ca b lc Kalman m rng

Cc thng s ci t

L thuyt v cc ng dng ca b lc KalmanPage 34

M phng qu trnh theo di mc tiu trn mng cm bin khng dy s dng b

lc Kalman m rng:
S cm bin: 300
2
Vng trin khai: 700x700 m

Nng lng mi nt: 1J

Khong cm bin: 70m
Khong truyn: 150m
Cc thng s ci t ging nh trn b lc Kalman,, ch khc cc thng s sau:
M hnh o lng:
z k =h [k , x k ]+ v k
z k =[ R k [ 1, j ] Rk [ 2, j ] R k [ m , j ] ]

(xx ) +( y y )
h [ k , x ] = (xx ) +( y y )
k

( x xm )2 +( y y m )2

Trong :
-

(x, y) l ta ca mc tiu j ti thi im k.

Hk

xi , yi

l ta ca sensor i bit

l Jacobi ca ma trn

h [ k , xk ]

hi 1=( x x1 )2+( y y 1)2

L thuyt v cc ng dng ca b lc KalmanPage 35

H
[ ]1 i m, 1 n 4
H k =

H =

hi 1
xx i
=
n=1
x ( xx i)2 +(x yi )2
hi 1
=0 n=2
x
y j ( k ) y i

hi 1
=
n=3
y ( x xi )2+(x y i )2
hi 1
=0 n=4
y

Kt qu ci t
-

Ngn ng s dng C++, s dng trnh m phng Omnet++ 4.2.2

Input: Qu o chuyn ng ca mc tiu

Output: Qu o c c lng.
Giao din m phng

L thuyt v cc ng dng ca b lc KalmanPage 36

 Kt qu nh gi
Qu o thc t

L thuyt v cc ng dng ca b lc KalmanPage 37

Qu o c c lng theo phng php bnh phng ti thiu

L thuyt v cc ng dng ca b lc KalmanPage 38

Qu o c c lng bng b lc Kalman m rng

L thuyt v cc ng dng ca b lc KalmanPage 39

IV.

KT LUN

V l thuyt

Nm r c b lc Kalman
C kh nng pht trin h thng c lng dng b lc Kalman
Hiu c cch m hnh ha h thng trong b lc Kalman
V ng dng
Bng nhng kin thc v b lc Kalman m phng c qu trnh c lng
chuyn ng ca mc tiu.
Hng pht trin
ng dng b lc Kalman vo h thng bt mc tiu trong mng cm bin khng
dy, gip qun l trng thi ca cc cm bin, tng cht lng theo di v ko
di thi gian sng ca mng.

L thuyt v cc ng dng ca b lc KalmanPage 40

TI LIU THAM KHO

[1] Gio trnh xc sut thng k, Tng nh Qu, NXB Gio dc, H Ni, 1999.
[2] Estimation II, Ian Ried-2001
[3] Tn la khng i khng, http://vi.wikipedia.org/wiki/Tn_la_khng_i_khng
[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 Tun, Nguyn Phng Quang, Ci thin cht lng truyn ng khng
ng b bng cu trc tch knh trc tip s dng kalman filter quan st t thng
-20-11-2009
[7] http://www.nchmf.gov.vn/web/vi-VN/71/155/5760/Default.aspx
[8] Ng Mnh Tin, Phan Xun Minh, H Th Kim Duyn, 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.

L thuyt v cc ng dng ca b lc KalmanPage 41