Laborgyakorlat

Kálmán szűrő algoritmus alkalmazása

dinamikus rendszerek állapotbecslésére

1. Bevezető
Jelen dolgozat célja röviden bemutatni a dinamikus rendszerek nem mérhető
állapotainak a becslésére használt Kálmán szűrő algoritmus két változatát. A rövid elméleti
bevezető után, külön tárgyalva lesz a lineáris rendszerekre alkalmazható egyszerű Kálmán
szűrő (KF- Kalman Filter) algoritmus, majd ezt követően a bővített Kálmán szűrő (EKF-
Extended Kalman Filter) tanulmányozása kerül sorra. Mindkét változatnál a szűrő
algoritmusoknak csak a diszkrét változata kerül bemutatásra. Az algoritmusok tesztelése és
ezek hatékonyságának vizsgálata különböző elméleti rendszerekre történő szimulációkkal
valósítható meg. Ezek a numerikus szimulációk Matlab környezetben vannak elkészítve.

2. Elméleti áttekintés és megközelítés

Az állapotbecslők feladata a rendszer állapotainak a meghatározása, akkor amikor az

állapotok nem mérhetők a kimeneten és/vagy az állapotokon jelenlévő zajok miatt. Ezért az
állapotbecslőket állapotszűrőknek is nevezhetjük, a cél ezeknek a zajoknak a hatását
eltávolítani vagy ezeknek a hatását csökkenteni a rendszeren belül. Egy dinamikus rendszer
állapotaira főleg egy állapot függő szabályozó alkalmazásánál van szükség, de még számos
más alkalmazás is szükségeltetheti a minél pontosabb állapot értékek meghatározását. Az 1.
ábrán szemléltetve van a Kálmán szűrő alkalmazásának az elvi összefüggése. Mint látható a
fizikai rendszeren megjelenő zajok (hibák vagy bizonytalanságok) két csoportba sorolhatók:
vannak az úgynevezett „rendszer dinamikáját közvetlenül befolyásoló” zajok (system noises)
és vannak a többségében mérésekből adódó zajok (measurements noises). A rendszeren
mérhető jelek:
- a bemenő jelek (input signal) – ezeket többnyire egy szabályozó/vezérlő egység
szolgáltatja a rendszernek;

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 - kimeneten mért jelek (measured signal) – melyet egy megfelelő mérőeszköz bocsájt a
felhasználó rendelkezésére.

System noises Measurements noises

Estimate
Input signals Measuring Observed signals Kalman of states
System device filter
System states
(not known)

Physical system

1. ábra A Kálmán szűrő algoritmus általános elvi alkalmazása

A cél egy olyan becslő meghatározása, mely a mért értékek alapján meghatározza a rendszer
nem-mérhető (vagy ismeretlen) állapotait egy bizonyos kritérium függvény optimalizálásával. A
Kálmán algoritmus a következő feltételek mellett alkalmazható:
- a rendszer illetve a kimeneten jelentkező zajok Gauss eloszlásúak, nulla középértékkel
és ismert kovariancia mátrixokkal;
- a dinamikus rendszer matematikai modellje ismert;
- a tanulmányozott folyamat megfigyelhető kell legyen (a rendszer kimenetén mért
jelekből megfigyelhetők kell legyenek a rendszer belső állapotainak a változásai).

3. Kálmán szűrő algoritmus

3.1. Egyszerű lineáris Kálmán szűrő algoritmus

Legyen a következő lineáris diszkrét állapotteres rendszermodell:
x k 1  Ak  x k  B k  u k  wk
y k  C k  xk  v k (1)
ahol x k   n - az n dimenziós állapotjel vektor;

u k   m - az m dimenziós bemenőjel vektor;

y k   p - a p dimenziós kimenőjel vektor;

wk   n az n dimenziós rendszerzaj vektor;

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 v k   p a p dimenziós kimeneten megjelenő zaj vektor;

Ak , B k , C k - a folyamatnak megfelelő rendszermátrixok.

A rendszerben megjelenő zajokra feltételezzük, hogy Gaussos fehér zaj szekvenciák

w ~ N (0,  w ) és v ~ N (0,  v ) (nulla középértékkel és ismert kovariancia mátrixokkal):

E{wk }  E{v k }  0 (2)

E{wk  wTk }  Q k E{v k  v Tk }  R k (3)

ahol E{} középértéket számoló operátor .
A kezdeti állapotvektor ugyancsak egy normál eloszlású véletlenszerű jel, melynek
középértéke és kovariancia mátrixa ismert. A bemenő jel determinisztikus diszkrét szekvencia.
A szűrő meghatározása az úgynevezett becslési hiba (a becsült állapot és a valós állapot közötti
különbség) által meghatározott kritérium függvény (négyzetes középérték) minimumának a
meghatározásán alapszik. A szűrő algoritmusának dinamikájában elkülöníthető két lépéses
ciklus:
- predikciós (jóslás) ciklus: meghatározza a k+1 – ik lépésben az állapotvektor értékét
úgy, hogy a kimeneten még nem jelent meg az yk+1 érték;
- javítás (update): módosítja a predikciós ciklusban kapott értéket, felhasználva már a
kimeneten megjelent yk+1 értéket is.
Bevezetetjük a következő jelöléseket:
xˆ k 1 / k  E{x k 1 Y k , U k } - a k+1 – ik lépésben előre becsült állapot

xˆ k 1 / k 1  E{x k 1 Y k 1 , U k } - a k+1 – ik lépésben javított becsült állapot

az előbbi jelölés használható a megfelelő becslési hiba kovariancia mátrixokra is. Ezeknek a
meghatározása is két lépésben történik:
Pk 1 / k  E{( x k 1  xˆ k 1 / k )  ( x k 1  xˆ k 1 / k ) T Y k , U k } - a k+1 – ik lépésben előre becsült kovariancia

mátrix
Pk 1 / k 1  E{( x k 1  xˆ k 1 / k 1 )  ( x k 1  xˆ k 1 / k 1 ) T Y k 1 , U k } a k+1 – ik lépésben javított becsült

kovariancia mátrix. Az Yk és az Uk a k-dik lépésig elvégzett kimeneti és bemeneti szekvenciákat

jelöli.
A szűrő dinamikája jellemezhető a következő differencia egyenlettel:
xˆ k 1 / k 1  xˆ k 1 / k  K k 1  ( y k 1  C k 1  xˆ k 1 / k )

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 ahol a Kk+1 a becslő erősítési mátrixa (mérete n x p).
Felhasználva a rendszer matematikai leírását és keresve a kovariancia mátrix minimális értéket
meghatározható a szűrő algoritmust jellemző rekurzív képletek. A már említett jelöléseknek
megfelelően az egyszerű diszkrét Kálmán szűrő a következő algoritmussal adható meg:
0. Kezdeti értékek megválasztása ( k  0 )
- becsült kezdeti állapot vektor xˆ k / k (mérete n x 1)
- a kezdeti becsült hiba kovariancia mátrixa Pk / k (mérete n x n)
1. Mérés: a bemenő (uk) és a neki megfelelő kimenő jel meghatározása (yk).
2. Predikció (jóslás):
xˆ k 1 k  Ak  xˆ k k  Bk  u k (4)

Pk 1 k  Ak  Pk k  Ak T  Q k (5)
3. A becslő erősítési vektor meghatározása:
T

K k 1  Pk 1 k C k C k Pk 1 k C k  R k
T

1
(6)

4. Javítás (update):

xˆ k 1 k 1  xˆ k 1 k  K k 1  y k 1  C k 1  xˆ k 1 k  (7)

Pk 1 k 1  I  K k 1  C k Pk 1 k (8)

5. Az algoritmus folytatódik az első lépéssel kezdődően k=k+1 –re.

Az előbbi algoritmus időben változó rendszerekre érvényes. Ha a lineáris dinamikus
rendszer időben invariáns (Ak=A, Bk=B és Ck=C, k  0 ) és a zaj szekvenciák kovariancia
mátrixai is időben állandók (Qk=Q és Rk=R), akkor a becslő dinamikája a következő
képletsorral jellemezhető (itt a becsült kovariancia mátrix és a becsült állapotvektor predikciós
és javítás lépései összevonva jelennek meg):
xˆ k 1 / k  A  xˆ k / k 1  B  u k  K k  ( y k  C  xˆ k / k 1 ) (9)

K k  Pk k 1C C  P C
T
k k 1
T
R 1
(10)

Pk 1 / k  A  Pk / k 1  A T  Q  A  K  C  P
k k / k 1 
 C T  R  K kT  A (11)
Megjegyzendő, hogy az eredeti rendszer dinamikája időben invariáns, de a Kálmán becslő egy
időben változó dinamikus rendszerként tekinthető (9 képlet), ahol a K erősítési mátrix mindig
függ a k-dik lépéstől.

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 Az algoritmus konvergenciája nagymértékben függ a kezdeti értékeknek a helyes
megválasztásától. Javasolt, hogy a kezdeti becsült állapotvektor legyen egyenlő a rendszer
valós kezdeti állapotvektorának az értékével. Ha viszont ez nem ismert, akkor érdemes ezt az
értéket nulláról indítani ( x0 / 0  0 ). A becsült hiba kezdeti kovarianciája P0 / 0    I , ahol az 
paraméter értéke jelöli azt, hogy mekkora a „bizonytalansága” a kezdeti becslő hibának. Ha a
becslő algoritmus konvergens, akkor a lépésről lépésre számított kovariancia mátrixnak a
nyoma csökkeni fog, mindez jelzi azt, hogy a becsült állapotvektor értékei a valós értékek felé
tartanak.

3.2. Bővített nemlineáris Kálmán szűrő algoritmus

A 3.1-es paragrafusban az az eset volt tárgyalva, amikor a dinamikus rendszer

(melynek állapotait becsülni szükséges) működése lineáris állapotteres matematikai modellel
írható le. Viszont a valóságban a valós folyamatok többsége nemlineáris viselkedéssel
jellemezhető és csak nagyon kis részük közelíthető meg lineáris matematikai modellekkel.
Legyen egy diszkrét dinamikus rendszer, mely általánosan a következő nemlineáris
állapotteres matematikai modellel jellemezhető:
xk 1  F ( xk , u k )  wk
(12)
yk  G ( xk , u k )  vk

ahol a képletben megjelenő mennyiségekre 2.1. –es paragrafusban már említett tulajdonságok
érvényesek. Az F: nn és G:pn pedig nemlineáris (de folytonos) többváltozós
függvények. A bővített Kálmán szűrő (EKF – Extended Kalman Filter) algoritmus hasonló az
egyszerű Kálmán szűrő algoritmushoz viszont itt minden lépésben (munkapontban) a
nemlineáris modell egy linearizált modellel helyettesíthető. Tehát szükséges az adott
munkapont körül a rendszer linearizálása. Ennek megfelelően az EKF algoritmus a következő
lépésekkel írható le:
0. Kezdeti értékek megválasztása ( k  0 )
- becsült kezdeti állapot vektor xˆ k / k (mérete n x 1)
- a kezdeti becsült hiba kovariancia mátrixa Pk / k (mérete n x n).
1. Mérés: a bemenő (uk) és a neki megfelelő kimenő jel meghatározása (yk).
2. Linearizálás az adott munkapont körül ( k  1 ): a nemlineáris matematikai modellben

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 szereplő F() és G() függvények linearizálásával a következő mátrixok számolhatók ki:
 F1 F1   G1 G1 
 x    x  x 
xn  1 n 
 1  (13)
Ak       Ck      
 Fn 
Fn  Gp Gp 
 x1 x n   x  x 
  x  xˆk / k  1 n  xxˆ
k /k

A liniarizálás a nemlineáris függvények Taylor sorba fejtésével történik, ahol csak az első tag
kerül megőrzésre.
3. Predikció (jóslás):
xˆ k 1 / k  F ( xˆk / k , uk ) (14)

Pk 1 k  Ak  Pk k  Ak T  Q k (15)

4. A becslő erősítési vektor meghatározása:

T

K k 1  Pk 1 k C k C k Pk 1 k C k  R k
T

1
(16)

5. Javítás (update):

xˆ k 1 k 1  xˆk 1 k  K k 1  yk 1  G ( xˆ k 1 / k , u k )  (17)

Pk 1 k 1  I  K k 1  C k Pk 1 k (18)

6. Az algoritmus folytatódik az első lépéssel kezdődően k=k+1 –re.

Látható, hogy az EKF algoritmusnál a második lépésben szükséges kiszámolni az F és G
függvények parciális deriváltjait. Numerikus alkalmazás esetén sok esetben a deriváltakat
különböző megközelítő módszerekkel lehet meghatározni. Az algoritmus konvergenciáját
befolyásolja ezek a megközelítő módszerek is.

4. Eset tanulmányok. Numerikus szimulációk.

Ebben a paragrafusban szimuláció szintjén bemutatásra kerülnek az elméletben
bemutatott állapotbecslő algoritmusok alkalmazásai különböző elméleti rendszerekre.
Feladat 1.
Adott a következő lineáris diszkrét dinamikus rendszer:
 0.4 0   0. 1 
xk 1     xk     uk  wk
 0 .01 0 .3   0.2  (19)
yk  0.3 0.2  xk  vk

A bemenő jel egy változó amplitúdójú négyszög jel (2.a ábra), a rendszeren lévő zajok pedig a
következő paraméterekkel jellemezhetők: w ~ N (0,0.03) illetve v ~ N (0,0.01) . A rendszer válasza
pedig ábrázolva van a 2.b. ábrán.

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 Input signal Output signal
1.5 0.2

0.15
1

0.1
0.5

0.05
Amplitude

Amplitude
0
0

-0.5
-0.05

-1
-0.1

-1.5 -0.15
0 100 200 300 0 100 200 300
No. of samples No. of samples

a). b).
2. ábra A rendszer bemenetén és kimenetén mért jelek

A becsléshez szükséges kezdeti értékek: xˆ 0 / 0  0 és P0 / 0  2  I 2 . A KF becslő algoritmus a

következő grafikus ábrákat eredményezi: 3-5 ábrák.

Variation of state x1
0.4
x1 without noise (unknown values)
0.2 x1 estimated
Amplitude

-0.2

-0.4
0 20 40 60 80 100 120 140 160 180 200
No of samples
Variation of state x1
0.4
x1 with noise
0.2 x1 estimated
Amplitude

-0.2

-0.4
0 20 40 60 80 100 120 140 160 180 200
No of samples

3. ábra Az x1 állapot ideális (nem ismert), becsült és zajos változása

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 A (3 és 4 ábrák) felső grafikonokon az ideális (ami nem ismert) és a becsült állapotok vannak
összehasonlítva mindkét állapotjel esetében. Ugyanezen ábrák alsó grafikonjain a becsült és
zajos állapotok vannak ábrázolva, itt megfigyelhető a tervezett becslő „szűrő” hatása is.

Variation of state x2
0.4
x2 without noise (unknown values)
0.2 x2 estimated
Amplitude

-0.2

-0.4
0 20 40 60 80 100 120 140 160 180 200
No of samples
Variation of state x2
0.4
x2 with noise
0.2 x2 estimated
Amplitude

-0.2

-0.4
0 20 40 60 80 100 120 140 160 180 200
No of samples

4. ábra Az x2 állapot ideális (nem ismert), becsült és zajos változása

Variation of output Trace of covariance matrices
0.15 4

0.1 3.5

3
0.05 measured output
estimated output 2.5
0
Amplitude

Amplitude

2
-0.05
1.5

-0.1
1

-0.15 0.5

-0.2 0
0 50 100 150 200 0 5 10 15 20
no of samples No of samples

a) b)
5. ábra A rendszer mért és becsült kimenete (a) illetve a becsült szórás mátrix nyoma

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 Feladat 2.
Adott a következő nemlineáris diszkrét dinamikus rendszer:
x1, k 1  x1,k  0.1  x 2,k
x 2,k 1  x 2,k  0.02  (1  x12, k )  x 2, k  0.1  ( x1,k  u k ) (20)
y k  x1,k  x 2, k

A bemenő jel egy változó amplitúdójú négyszög jel (6.a. ábra), a rendszeren lévő zajok pedig a
következő paraméterekkel jellemezhetők: w ~ N (0,0.2) illetve v ~ N (0,0.1) . A rendszer válasza
pedig ábrázolva van a 6.b. ábrán.
Input signal Output signal
1.5 3

1 2

0.5 1
Amplitude

Amplitude
0 0

-0.5 -1

-1 -2

-1.5 -3
0 100 200 300 0 100 200 300 400
No. of samples No. of samples

a). b).
6. ábra A rendszer bemenetén és kimenetén mért jelek

A becsléshez szükséges kezdeti értékek: xˆ 0 / 0  0 és P0 / 0  5  I 2 . Az EKF becslő algoritmus a

KF algoritmushoz hasonlóan a következő grafikus ábrákat eredményezi:7-8 ábrák.

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 Variation of state x1
4
x1 without noise (unknown values)
2 x1 estimated

Amplitude
0

-2

-4
0 0.5 1 1.5 2 2.5 3
No of samples
Variation of state x1
4
x1 with noise
2 x1 estimated
Amplitude

-2

-4
0 0.5 1 1.5 2 2.5 3
No of samples

7. ábra Az x1 állapot ideális (nem ismert), becsült és zajos változása

Variation of state x2
2
x2 without noise (unknown values)
1 x2 estimated
Amplitude

-1

-2
0 0.5 1 1.5 2 2.5 3
No of samples
Variation of state x2
2
x2 with noise
1 x2 estimated
Amplitude

-1

-2
0 0.5 1 1.5 2 2.5 3
No of samples

8. ábra Az x2 állapot ideális (nem ismert), becsült és zajos változása

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.
 Variation of output Trace of covariance matrices
3 10
measured output
estimated output 9
2
8

7
1
6
Amplitude

Amplitude
0 5

4
-1
3

2
-2
1

-3 0
0 1 2 3 0 5 10 15 20
no of samples No of samples

9. ábra A rendszer mért és becsült kimenete (a) illetve a becsült szórás mátrix nyoma
5. Gyakorlati megvalósítás
5.1 Végezzék el a 4. alpontban bemutatott rendszerek állapotbecsléssel kapcsolatos szilációkat
Matlab környezetben (program).
5.2 A szimulált algoritmusokat valósítsák meg Matlab-Simulink környezeteben is. Az
állapotbecslő algoritmusok S-function használatával valósítható meg.

Könyvészet:

[1] M. I. Riberio, (2004), Kalman and Extended Kalman Filters, Concept, Derivation and
Properties, Instituto Superior Tecnico,. (http://users.isr.ist.utl.pt/~mir/pub/kalman.pdf)
[2] Huang, S., Understanding Extended Kalman filter, ARC Centre of Excellence for
Autonomous Systems (CAS), University of Technology Sydney 2010
(http://services.eng.uts.edu.au/~sdhuang/Extended%20Kalman%20Filter_Shoudon).
[3] http://www.ece.iastate.edu/~namrata/EE527_Spring08/KalmanFilter.pdf
[4] https://dspace.ist.utl.pt/bitstream/2295/203912/1/Cap4-Kalman-Filter%20

Created with novaPDF Printer (www.novaPDF.com). Please register to remove this message.