Kalmanův filtr

Obsah
Kalman˚uv filtr
Moderní teorie ˇrízení
1 Úvod 1
2 Pˇríklady 6
2.1 Praktick´y návrh Kalmanova filtru . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.1 ˇ Reˇsení pomocí Matlabu . . . . . . . . . . . . . . . . . . . . . . . . . 9
3 Domácí úloha 11
Reference 11
1 Úvod
Kalman˚uv filtr – pomocí Wienerova filtru [1] lze ˇreˇsit problém optimální filtrace pro
signály, jejichˇz vlastnosti a vzájemné vztahy byly charakterizovány autokorelačními funk-
cemi Pxx(k), Pyy(k) a vzájemnou kovarianční funkcí Pxy(k). Tento popis je moˇzné chápat
jako vnějˇsí popis.
Nyní vyuˇzijeme vnitˇrního (stavového) popisu. Podmínkou ˇreˇsitelnosti úloh filtrace, pre-
dikce a interpolace, formulovan´ych pomocí vnitˇrního popisu systému, je moˇznost odhadovat
stav systému na základě pozorování vstup˚u a v´ystup˚u systému. V deterministické formulaci
problému lze k odhadování stavu sestrojit pozorovatel stavu metodou umíst’ování pól˚u. Ve
stochastické formulaci problému lze úlohu odhadu stavu formulovat ve smyslu optimálního
LMS odhadu stavu a v´ysledn´y optimální pozorovatel stavu se naz´yvá Kalman˚uv filtr.
⋆ LMS odhad – lineární odhad minimalizující stˇrední kvadratickou chybu. Kritérium je
JLMS = E ˜x T ˜x , ˜x = x − ˆxLMS , (1)
kde pˇredpokládáme, ˇze hledaná data ˆxLMS(y) jsou pouze afinní funkcí měˇren´ych dat y
Dosadíme do kritéria
ˆxLMS(y) = Ay + b . (2)
E ˜x T ˜x = E trace (˜x T ˜x) = E trace (˜x˜x T ) = trace (E ˜x˜x T ),
1

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 2
kde trace (A) – stopa matice A je součet prvk˚u na diagonále matice A. Tento operátor
komutuje s operátorem stˇrední hodnoty, stopa skaláru je skalár sám a ve stopě součinu matic
je moˇzno matice cyklicky zaměňovat.
Dále pokračujeme
T
JLMS = trace E (x − Ay − b) (x − Ay − b) =
= trace E{xxT − Ayx T − bx T − xyT A T +
+Ayy T A T + by T A T − xb T + Ayb T + bb T
} =
= trace µxµ T x + P xx − Aµyµ T x − AP yx − bµ T x − µxµ T y A T − P xyA T +
+A µyµ T T
y + P yy A + bµ T y A T − µxb T + Aµyb T + bb T
=
= trace µxµ T x + P xx − 2Aµyµ T x − 2AP yx − bµ T x +
+A µyµ T T
y + P yy A − µxb T + 2Aµyb T + bb T
=
= trace P xx + A P yy + µyµ T T
y A + (b − µx)(b − µx) T + 2Aµy(b − µx) T
− 2AP yx .
S vyuˇzitím vztah˚u pro derivace stopy matice
dostaneme
Odtud dostaneme
∂
∂Y trace XY Z = X T Z T ,
∂
∂X trace XY X T = 2XY ,
∂
∂A JLMS = 2A P yy + µyµ T
y + 2 (b − µx) µ T y − 2P xy = 0 ,
∂
∂b JLMS = 2 (b − µx) + 2Aµy = 0 .
A = P xyP −1
yy ,
b = µx − P xyP −1
yy µy .
⋆ Optimální lineární odhad minimalizující stˇrední kvadratickou chybu je tedy dán vztahem
ˆxLMS(y) = µx + P xyP −1
yy (y − µy) . (3)
D˚uleˇzitou vlastností odhadu ˆxLMS je právě skutečnost, ˇze závisí pouze na prvních dvou mo-
mentech – stˇredních hodnotách µx, µy a kovariančních maticích P xy, P yy. Platí samozˇrejmě
vlastnost E {ˆxLMS(y)} = µx. Kovarianční matici m˚uˇzeme odvodit následovně
x = E − ˆxLMS(y) x − ˆxLMS(y) T
=
= E
P ˜xLMS (x − µx) − P xyP −1
yy (y − µy) (x − µx) − P xyP −1
yy (y − µy) T
=
= P xx − P xyP −1
yy P yx − P xyP −1
yy P yx + P xyP −1
yy P yyP −1
yy P yx .
⋆ Kovarianční matice chyby odhadu tedy je
P ˜xLMS = P xx − P xyP −1
yy P yx . (4)

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 3
⋆ Stochastick´y systém – lineární, diskrétní, pozorovateln´y
x(k + 1) = Mx(k) + Nu(k) + v(k) (5)
y(k) = Cx(k) + Du(k) + e(k) ,
kde ˇsum procesu v(k) a ˇsum měˇrení e(k) jsou diskrétní bílé posloupnosti s nulovou stˇrední
hodnotou
E
v(k)
e(k)
= 0 (6)
a známou kovarianční maticí
⎧
T
⎨
v(k1) v(k2)
E
⎩ e(k1) e(k2)
⎫ ⎬
⎭ =
Q S
S T
= 1 k1 = k2
δ(k1 − k2) , δ(k1 − k2)
. (7)
R
= 0 k1 = k2
Z (7) vidíme, ˇze jsou ˇsumy navzájem korelované, ale v r˚uzn´ych časev´ych okamˇzicích jsou jiˇz
nezávislé. Kovarianční matice jsou symetrické a uvaˇzujeme Q ≥ 0, R > 0.
Pˇredpokládejme, ˇze v k-tém kroku algoritmu známe apriorní odhad stavu x(k) (tj. odhad
vyuˇzívající data aˇz do času k−1, ale neberoucí v úvahu data y(k)), jehoˇz podmíněnou stˇrední
hodnotu budeme značit
ˆx(k| k − 1)
pˇritom první index označuje běˇzn´y čas a druh´y index poslední měˇrení, které je uˇzito k určení
odhadu. Kovarianční matici chyby odhadu stavu budeme značit
P (k| k − 1) .
Počáteční stav systému je tedy charakterizován stˇrední hodnotou ˆx(0) = ˆx(0| − 1) a kova-
rianční maticí P (0) = P (0| − 1).
Po změˇrení hodnoty v´ystupu y(k) chceme tyto hodnoty aktualizovat a získat aposteriorní
odhad stavu (tj. odhad zahrnující měˇrení y(k))
ˆx(k| k)
a pˇrísluˇsnou kovarianční matici chyby odhadu
P (k| k).
Kalman˚uv filtr pro nekorelované ˇsumy procesu a měˇrení – v tomto pˇrípadě je
matice S = 0.
Nejprve odvodíme v´yvoj stˇrední hodnoty
ˆx(k + 1| k − 1) = E {Mx(k) + Nu(k) + v(k)} = M ˆx(k| k − 1) + Nu(k)
ˆy(k| k − 1) = E {Cx(k) + Du(k) + e(k)} = C ˆx(k| k − 1) + Du(k) ,
(8)

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 4
její chyby
˜x(k + 1| k − 1) = x(k + 1) − ˆx(k + 1| k − 1) = M ˜x(k| k − 1) + v(k)
˜y(k| k − 1) = y(k) − ˆy(k| k − 1) = C ˜x(k| k − 1) + e(k)
a kovarianční matice chyby odhadu
⎧
T
⎨
˜x(k + 1) ˜x(k + 1)
E
⎩ ˜y(k) ˜y(k)
⎫
⎬
k − 1
⎭ =
P xx
P yx
P xy
P yy
. (10)
Odvodíme P xx
T
P xx = E [˜x (k + 1| k − 1)] [˜x (k + 1| k − 1)] =
T
= E [M ˜x(k| k − 1) + v(k)] [M ˜x(k| k − 1) + v(k)] =
= E M ˜x(k| k −1)˜x T (k| k −1)M T +v(k)˜x T (k| k −1)M T +M ˜x(k| k −1)v(k)+v(k)v T (k) =
= MP (k| k − 1)M T + 0 + 0 + Q .
Obdobně získáme P xy, P yx a P yy
P xx
P yx
P xy
P yy
T
MP (k| k − 1)M + Q
=
CP (k| k − 1)M
T
MP (k| k − 1)C + S
T + S T CP (k| k − 1)C T
.
+ R
(11)
ˇRekli jsme, ˇze pro nekorelované ˇsumy procesu a měˇrení je S = 0. Dosadíme-li nyní do (3),
získáme vztah pro odhad stavu x(k)
ˆx(k + 1| k − 1) = M ˆx(k| k − 1) + Nu(k)+
+MP (k| k − 1)C T CP (k| k − 1)C T + R −1 [y(k) − C ˆx(k| k − 1) − Du(k)].
Dosazením do (4) získáme Riccatiho rovnici pro Kalman˚uv filtr
P (k + 1| k − 1) = MP (k| k − 1)M T + Q −
− MP (k| k − 1)C T CP (k| k − 1)C T + R −1 CP (k| k − 1)M T .
Rovnice (12) a (13) pˇredstavují algoritmus Kalmanova filtru a dají se rozdělit do dvou
nezávisl´ych krok˚u [1].
⋆ Datov´y (filtrační) krok Kalmanova filtru
kde ε(k| k − 1) je chyba odhadu v´ystupu
ˆx(k| k) = ˆx(k| k − 1) + L ′ (k) ε(k| k − 1) ,
P (k| k) = P (k| k − 1) − L ′ (k)CP (k| k − 1) ,
ε(k| k − 1) = y(k) − C ˆx(k| k − 1) − Du(k)
(9)
(12)
(13)
(14)

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 5
a L ′ (k) je Kalmanovo zesílení datového kroku
L ′ (k) = P (k| k − 1)C T CP (k| k − 1)C T + R −1 . (15)
⋆ Časov´y (predikční) krok Kalmanova filtru
ˆx(k + 1| k) = M ˆx(k| k) + Nu(k) ,
P (k + 1| k) = MP (k| k)M T + Q .
Algoritmus Kalmanova filtru je znázorněn na obr. 1. Poznmenejme, ˇze tento obrázek je pouze
ilustrativní – datov´y krok Kalmanova filtru je moˇzné provést aˇz po změˇrení nov´ych dat, tedy
časov´y krok nemá dva mezikroky jak by se podle obrázku na první pohled zdálo.
x(k-1|k-2)
u(k-1)
y(k-1)
x(k-1|k-1)
x(k|k-1)
u(k)
y(k)
èasový krok
x(k|k)
datový krok
Obrázek 1: Cyklus Kalmanova filtru
Uvědomme si, ˇze Kalmanovo zesílení celého filtru je
x(k+1|k)
èas - t
(16)
L(k) = ML ′ (k). (17)
Dále si vˇsimněme toho, ˇze pokud budou ˇsumy procesu a měˇrení korelované (S = 0), nep˚ujde
algoritmus Kalmanova filtru (rovnice (12) a (13)) rozdělit na datov´y a časov´y krok. V [1]
je vˇsak ukázáno, ˇze úlohu Kalmanova filtru pro korelované ˇsumy procesu a měˇrení lze vˇzdy
pˇrevést na úlohu Kalmanova filtru pro nekorelované ˇsumy procesu a měˇrení.
Kalman˚uv filtr je tedy algoritmus generující posloupnost odhad˚u stavu ˆx(k|k) a kova-
riančních matic chyb odhadu P (k|k), pˇričemˇz odhad stavu ˆx(k|k) v kaˇzdém kroku minima-
lizuje kritérium
JLMS = trace P (k|k). (18)

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 6
2 Pˇríklady
2.1 Praktick´y návrh Kalmanova filtru
Pˇríklad 2.1: Pro spojitou soustavu
A =
,
B =
000
0 1 0 0
0 0 1 0
0 0 0 1
−10 −11 −24010 −2539 −54
31940
, C = [ 1 0 0 0 ], D = [0]
navrhněte časově invariantní Kalman˚uv filtr. Periodu vzorkování volte Ts = 0,01 s. Pˇredpo-
kládejte nekorelované ˇsumy procesu a měˇrení.
ˇReˇsení: Systém je pozorovateln´y, takˇze má smysl pokračovat v návrhu. Nejprve pro-
vedeme diskretizaci, abychom získali matice M a N. Návrh Kalmanova filtru spočívá ve
vyˇreˇsení Riccatiho rovnice (13). Tuto rovnici budeme ˇreˇsit sekvenčně ve směru času (na
rozdíl od Riccatiho rovnice pro LQ regulátor, kterou jsme ˇreˇsili zpětně v čase).
Abychom mohli Kalman˚uv filtr navrhnout, musíme znát parametry ˇsumu procesu a ˇsumu
měˇrení (kovarianční matice těchto ˇsum˚u Q a R), nebo je nějak odhadnout. Pˇredpokládejme
Q = diag [ 0,01; 0,01; 0,1; 0,1 ] , R = [ 0,05 ].
Budeme tedy iteračně ˇreˇsit rovnici (13) dokud nedosáhneme ustáleného Kalmanova zesílení
(podobně jako u LQ regulátoru). Toto ustálené Kalmanovo zesílení pouˇzijeme pro časově
invariantní filtr (viz obr. 6).
L
K
0.5
0
−0.5
−1
−1.5
−2
Kalmanovo zesileni L
−2.5
0 50 100 150 200 250 300
0.5
0
−0.5
−1
−1.5
Kalmanovo zesileni K
−2
0 50 100 150
Diskretni cas − k
200 250 300
Obrázek 2: Pr˚uběh Kalmanova zesílení

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 7
x 1
err
y
err
4
2
0
Skutecny stav systemu
−2
0
5
1 2
Korelace
3
0
−5
−400
0.5
−200 0 200
Chyba odhadu
400
0
−0.5
0 1 2 3
Cas (s)
5
0
x 1p
x 1 , x 1p
Obrázek 3: Pr˚uběh odhadu stavu x1
Vystup systemu
−5
0
40
1 2
Korelace
3
20
0
−20
−400
1
−200 0 200
Chyba odhadu
400
0
−1
0 1 2 3
Cas (s)
y p
y, y p
4
2
0
Odhad stavu
−2
0
10
1 2
Histogram chyby odhadu
3
5
0
−0.4
4
−0.2 0 0.2
Stav a jeho odhad
0.4
2
0
−2
0 1 2 3
Cas (s)
4
2
0
Odhad vystupu
−2
0
10
1 2
Histogram chyby odhadu
3
5
0
−1
5
−0.5 0 0.5
Vystup a jeho odhad
1
0
−5
0 1 2 3
Cas (s)
Obrázek 4: Pr˚uběh odhadu v´ystupu y

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 8
Pokud jsme navrhli Kalman˚uv filtr správně, musí b´yt chyba predikce (odhadu) v´ystupu bíl´y
ˇsum. O tom se m˚uˇzeme pˇresvědčit na obr. 4.
Zesílení (dB)
Fáze (°)
−20
−40
−60
−80
−100
−120
−140 0
−90
−180
−270
−360
10 −1
−450
10 0
Prenos U −> odhad X 1
10 1
Frekvence (rad/s)
10 2
10 3
Zesílení (dB)
Fáze (°)
0
−5
−10
−15 0
−45
−90
−135
−180
10 0
−225
Prenos Y −> odhad X 1
10 1
Frekvence (rad/s)
Obrázek 5: Frekvenční vlastnosti časově invariantního Kalmanova filtru
Obrázek 6: Simulinkové schéma časově invariantního Kalmanova filtru
Na obr. 6 vidíme, ˇze časově invariantní Kalman˚uv filtr má stejnou strukturu jako pozoro-
vatel stavu. Matice L je ovˇsem navrˇzena optimálně pro kovariance ˇsumu procesu Q a ˇsumu
měˇrení R. ✷
10 2
10 3

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 9
2.1.1 ˇReˇsení pomocí Matlabu
%----- KALMANUV FILTR
randn(’state’,0);%reset generatoru nahodnych cisel
%----- Soustava
load system.mat;
[Ad, Bd, Cd, Dd] = ssdata(c2d(ss(Ac,Bc,Cc,Dc),Ts));
Ns = size(Ad,1); Ny = size(Cd,1);
%----- Kovariancni matice
Qd = diag([0.01, 0.01, 0.1, 0.1]); Rd = 0.05;
Pd = 1000*Qd;%pocatecni hodnota kovariancni matice
%----- Pocatecni podminky
xs = zeros(Ns,1); xp = zeros(Ns,1);
%----- Inicializace historie
N = 300;
uh = [1*ones(1,N)]; eh = zeros(1,N);
xsh = zeros(Ns,N); xph = zeros(Ns,N);
Lh = zeros(Ns,N); Kh = zeros(Ns,N);
ysh = zeros(1,N); yph = zeros(1,N);
%----- Simulace
for t = 1 : N
end
v = chol(Qd)’*randn(Ns,1);
e = chol(Rd)’*randn(1,1);
u = uh(t);
ys = Cd*xs + Dd*u + e;%vystup systemu ys(k)
%----- Datovy krok KF
yp = Cd*xp + Dd*u;%odhad vystupu yp(k|k-1)
ep = ys - yp;%chyba odhadu vystupu e(k|k-1)
L = Pd*Cd’*inv(Cd*Pd*Cd’+Rd);%Kalmanovo zesileni
Pd = Pd - L*Cd*Pd;%kovariancni matice P(k|k)
xp = xp + L*ep;%odhad stavu x(k|k)
yp = Cd*xp + Dd*u;%odhad vystupu y(k|k)
%----- Ulozeni dat
eh(t) = ep; xsh(:,t) = xs; xph(:,t) = xp; ysh(t) = ys;
yph(t) = yp; Lh(:,t) = L; Kh(:,t) = Ad*L;
xs = Ad*xs + Bd*u + v;%stav systemu x(k)
%----- Casovy krok KF
xp = Ad*xp + Bd*u;%odhad stavu x(k+1|k)
Pd = Ad*Pd*Ad’ + Qd;%kovariancni matice P(k+1|k)

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 10
%----- Prubehy stavu
DobaC = (1:N)*Ts;
err = xsh - xph(1:Ns,:);
figure(1);
subplot(3,2,1);
plot(DobaC,xsh(1,:)’,’b’);
ylabel(’x1’);title(’Skutecny stav systemu’);
current=axis;
subplot(3,2,2);
plot(DobaC,xph(1,:)’,’r’);
ylabel(’x1p’);title(’Odhad stavu’);
axis(current);
[C,LAGS]=xcorr(err(1,:));
subplot(3,2,3);
plot(LAGS,C);
title(’Korelace’);
subplot(3,2,4);
hist(err(1,:),N/2);
title(’Histogram chyby odhadu’);
subplot(3,2,5);
plot(DobaC,err(1,:)’);
xlabel(’Cas (sec)’);ylabel(’err’);title(’Chyba odhadu’);
subplot(3,2,6);
plot(DobaC,xsh(1,:)’,’b’);
hold on;
plot(DobaC,xph(1,:)’,’r’);
xlabel(’Cas (sec)’);ylabel(’x1, x1p’);title(’Stav a jeho odhad’);
%-----Frekvencni vlastnosti invariantniho KF
K = Ad*Lh(:,end);
Ak = Ad - K*Cd;
Bk = [Bd - K*Dd,K];
Ck = eye(Ns); Dk = zeros(Ns,2);
[NumKU DenK] = ss2tf(Ak,Bk,Ck,Dk,1);%prenos U -> odhad X
[NumKY DenK] = ss2tf(Ak,Bk,Ck,Dk,2);%prenos Y -> odhad X
figure(Ns+3);
bode(tf(NumKU(1,:),DenK,Ts));
title(’Prenos U -> odhad X 1’);
figure(Ns+4);
bode(tf(NumKY(1,:),DenK,Ts));
title(’Prenos Y -> odhad X 1’);

MODERNÍ TEORIE ˇ RÍZENÍ – Kalman˚uv filtr 11
3 Domácí úloha
Pˇríklad 3.1: Proved’te diskretizaci (Ts = 0,01 s) systému z pˇríkladu 2.1. Systém simu-
lujte jako stochastick´y. Pro r˚uzné kovarianční matice Q a R navrhněte časově invariantní
Kalman˚uv filtr a porovnejte jeho frekvenční vlastnosti a vlastní čísla matice (M − LC)
v závislosti na kovariančních maticích Q a R. V´ysledky ověˇrte simulacemi v Matlabu.
Navrhněte Kalman˚uv filtr pro kovarianční matice Q a R. Stochastick´y systém ale simu-
lujte s jin´ymi kovariancemi ˇsum˚u. Co pˇri simulacích pozorujete?
Dále jen pro dobrovolníky. Pouˇzijte pro ˇrízení systému LQ regulátor a diskutujte rozdíly
pˇri regulaci, pouˇzijeme-li identick´y pozorovatel stavu nebo v´yˇse navrˇzen´y Kalman˚uv filtr.
Reference
[1] Havlena, V. Moderní teorie ˇrízení – Doplňkové skriptum. Praha: Vydavatelství
ČVUT, 1999. ISBN 80-01-02036-3
[2] Havlena, V.; ˇ Stecha, J. Moderní teorie ˇrízení. Praha: Vydavatelstní ČVUT, 2000.
[3] Roubal, J.; Pekaˇr, J. Moderní teorie ˇrízení [online]. Poslední revize 2005-01-16
[cit. 2005-01-16], 〈http://dce.felk.cvut.cz/mtr/〉.