Úvod do Kalmanova filtru Kalmanův filtr = odhadovač stavu systému KF dává dohromady model systému a měření. Model systému použije k tomu, aby odhadl, jak bude stav vypadat a poté stav porovná se skutečným měřením, a upraví ho tak, aby měření odpovídal Funguje jen pro lineární systémy (ale existují rozšíření pro nelineární). Rudolf Kalman Model systému (lineární)
x k +1 = A k x k + B k u k + v k
= y k H k xk + w k x k je vektor stavů o n prvcích, u k je vektor vstupů o m prvcích a y k je vektor výstupů o p prvcích. Matice A k je tedy rozměru
n × n , matice B k rozměru n × m , matice H k rozměru p × n v k je procesní šum, který zahrnuje poruchy, které ovlivňují dynamiku systému, ale nejsou modelovány, a w k je šum (porucha)
měření. Vektor v k má stejný počet prvků jako vektor stavu x k , vektor w k má stejný počet prvků jako výstupní vektor y k Předpoklady • v k má Gaussovo rozložení s nulovou střední hodnotou E ( v k ) = 0 a kovarianční maticí Pk = Vk wk
má Gaussovo rozložení s nulovou střední hodnotou E ( w k ) = 0 a kovarianční maticí
Pk = Wk
Co chceme zjistit? Odhad stavu. Odhad stavu bude mít Gaussovo rozložení se střední hodnotou xˆ k a kovarianční maticí Pk
Jak to funguje? Predikce
počáteční odhad
= xˆ k +1 k A k xˆ k k + B k u k = Pk +1 k A k Pk k ATk + Vk
Predikce akce → model
Korekce
xˆ k +1 k , Pk +1 k měření
Korekce měření → odhad
xˆ k +1 k +1 , Pk +1 k +1
xˆ k += xˆ k +1 k + K k +1y k +1 1 k +1 Pk += Pk +1 k − K k +1H k +1Pk +1 k 1 k +1 kde
y= y k +1 − H k +1xˆ k +1 k k +1
= S k +1 H k +1Pk +1 k HTk +1 + Wk +1 K k +1 = Pk +1 k HTk +1S k−1+1
Jednoduchý příklad Těleso padá z určité výšky, bez odporu vzduchu, jediná působící síla je gravitace. Máme k dispozici měřák výšky, který neměří příliš dobře. Chceme znát polohu a rychlost tělesa. Co bude ve stavu? To co chci určit, tedy výška a rychlost x = [ y
y ]
T
Jak bude vypadat model?
y (t ) = − g y ( t ) =y 0 − g ( t − t0 ) g 2 ( t − t0 ) 2 diskrétně pro přírůstek ( t − t0 ) = 1 g y ( k + 1= ) y ( k ) + y ( k ) − 2 vstupem je tíhové zrychlení, u = [ − g ] , které je pro všechny časové kroky konstantní výstupem je výška (výstupem musí být veličina, kterou přímo měříme) vyrobíme tedy příslušné matice y ( t ) = y0 + y 0 ( t − t0 ) −
v1k 1 1 yk 0.5 g + + = + − + x k += A x B u v [ ] 1 k k k k k v 0 1 y 1 k 2k y y k= H k x k + w k= [1 0] k + [ wk ] y k
v1k 1 1 yk 0.5 g + + = + − + x k += A x B u v 1 k k k k k 0 1 y 1 [ ] v k 2k y y k= H k x k + w k= [1 0] k + [ wk ] y k matice v k a w k neznáme, matice A,B,H jsou konstantní (v tomto příkladu!) a index k u nich není třeba Abychom mohli implementovat Kalmanův filtr, potřebujeme ještě znát matice Vk a Wk +1 . Co to je? Jsou to jediné parametry, které ve skutečnosti můžeme měnit. Jedná se o kovarianční matice, naše odhady procesního šumu a šumu měření. Když bude velká matice Vk , znamená to, že modelu příliš nevěříme. Když bude velká matice Wk +1 , znamená to, že nevěříme měření. Konkrétně jsou v těchto maticích rozptyly a kovariance příslušných šumů. Rozměry matic v našem konkrétním příkladu jsou:
0.5 0 Vk = a Wk +1 = [1] 0 0.5 A můžeme již celý příklad implementovat v Matlabu
1. Příprava % Kalmanuv filtr - padajici teleso clear all skutecna_poloha = 100; skutecna_rychlost = 0; g = 1; % velmi mala gravitace A = [1 1; 0 1]; B = [0.5; 1]; H = [1 0];
% matice soustavy % vliv akce % matice mereni
% odhady pocatecniho stavu pocatecni_poloha = 95; % odhadneme o 5 metru nize pocatecni_rychlost = 1; % odhadneme nenulovou rychlost (smerem nahoru) x0 = [pocatecni_poloha; pocatecni_rychlost]; % prvotni odhad stavu P0 = [0 0; 0 0]; % pocatecni kovariancni matice uk = -g; % akce je porad stejna V = [0.5 0; 0 0.5]; % odhad procesniho sum - timhle urcujeme jak dokonaly model mame W = [1]; % odhad sumu mereni - timhle urcujeme jak moc dobry merak mame
2. Vlastní simulace + výpočet Kalmanova filtru pocet_iteraci = 10; xhat = x0; % inicializace odhadu pocatecni hodnotou P = P0; % inicializace kovariancni matice pocatecni hodnotou for i= 1:pocet_iteraci % pro porovnani potrebujeme skutecne hodnoty skutecna_rychlost = skutecna_rychlost - g; skutecna_poloha = skutecna_poloha + skutecna_rychlost; % provedeme simulovane mereni sum = 1*randn; % sum mereni ymeasured = skutecna_poloha + sum; % predikce xhatk1k = A*xhat+B*uk; % predikce stavu Pk1k = A*P*A' + V; % predikce kovariancni matice % korekce yhat = H * xhatk1k; % jaky vypada mereni, kdyz je stav takovy, jaky predikujeme yvlnka = ymeasured - yhat; % rozdil mezi skutecnym merenim a predikovanym S = H*Pk1k*H' + W; % measurement prediction covariance K = P*H'*inv(S); % Kalmanovo zesileni xhatk1k1 = xhatk1k + K * yvlnka; % korekce stavu Pk1k1 = Pk1k - K*S*K'; % korekce kovariancni matice % a ted zalogujeme vsechna data a do odhadu a kovariancni matice dame % nove vypoctene hodnoty xhat = xhatk1k1; P = Pk1k1; log_poloha(i) = skutecna_poloha; log_rychlost(i) = skutecna_rychlost; log_odhadpolohy(i) = xhat(1); log_odhadrychlosti(i) = xhat(2); log_mereni(i) = ymeasured; end
3. Vykreslení výsledků % vykresleni figure subplot(1,2,1) hold on plot(log_poloha,'r'); plot(log_odhadpolohy,'b'); plot(log_mereni,'k'); legend('skutecnost','odhad','zmereno'); title('poloha'); subplot(1,2,2) hold on plot(log_rychlost,'r'); plot(log_odhadrychlosti,'b'); legend('skutecnost','odhad'); title('rychlost');
Co se stane, když budeme věřit modelu a nebudeme věřit měření? A jak toho docílíme? V = [0.01 0; 0 0.01]; % odhad procesniho sum W = [10.0]; % odhad sumu mereni
snížením odhadu procesního šumu věříme hodně modelu, zvýšením odhadu šumu měření nevěříme měřáku i když je model v pořádku, malou důvěrou k měření bude trvat poměrně dlouho, než se odhad přiblíží ke skutečnosti (proč tomu tak je, i když model přesně odpovídá ?)
Co se stane, když budeme přehnaně věřit měření? Pro lepší viditelnost je zvýšen šum na měřáku. V = [0.5 0; 0 0.5]; % odhad procesniho sum W = [0.0000001]; % odhad sumu mereni (radek 33)
sum = 10*randn;
% sum mereni
Výsledný odhad neuvažuje šum měření, tedy blíží se k tomu, co přesně je naměřeno
Skvělá vlastnost Kalmanova filtru – fůze dat z více zdrojů Upravíme příklad tak, aby bylo možné využít dvou nezávislých měřáků výšky, jednoho lepšího (s menším rozptylem) a druhého horšího. Co se změní ? Predikce zůstane stejná, změní se pouze korekce. Protože v modelu systému x k +1 = A k x k + B k u k + v k
= y k H k xk + w k se změní rozměr výstupu (teď budou dva), musí se změnit i matice H . A samozřejmě také matice Wk +1 , která nyní bude obsahovat rozptyly měření na hlavní diagonále. Mimo diagonálu jsou její prvky nulové (proč?) Úkol, upravte implementovaný příklad na použití dvou (tří) měřáků. malá nápověda:
1 0 H= 1 0 pokud první měřák bude mít rozptyl např. 1 a druhý 2 (větší rozptyl = horší měřák), bude
1 0 Wk +1 = 0 2
A co když bude mít měření nenulovou střední hodnotu chyby? Jak se vypořádat s offsetem? Řešení je jednoduché, zkrátka přidáme offset jako další proměnnou do stavu. Úkol, upravte vylepšený příklad s využitím dvou měřáků na odhad offsetu.
x k +1 = A k x k + B k u k + v k malá nápověda, připomeňme si model systému: = y k H k xk + w k stav se změní o offsety, tedy x = [ y
y o1 o2 ]
T
tím pádem se musí změnit i všechno ostatní, konkrétně především:
1 0 Ax k = 0 0
1 1 0 0
0 0 1 0
0 yk 0 y k 1 0 1 0 , Hk = 1 0 0 1 0 o1k 1 o2 k
Co když je systém, jehož stav odhadujeme, nelineární? Kalmanův filtr je pořád ten nejlepší lineární odhad, co existuje. Ale lineární odhad nelineárního systému bude vždy "nic moc". Upravme základní příklad s padajícím tělesem tak, že do něj zavedeme odpor vzduchu, jako lineární funkci kvadrátu rychlosti, tedy
y ( t ) =− g + cy 2 pro tuto chvíli nebudeme řešit signum (odpor je vždy proti směru rychlosti). ve zdrojovém kódu tedy upravíme cy = 0.01;
% odpor vzduchu (funkce kvadratu rychlosti)
a pro simulační výpočet skutecna_rychlost = skutecna_rychlost - g +
cy*skutecna_rychlost^2;
Zbytek ponechme beze změny, tedy použijeme lineární Kalmanův filtr pro nelineární soustavu a podíváme se, co se stane.
Vliv nelinearity roste se zvyšující se rychlostí (aby ne, když je závislá na kvadrátu rychlosti). Co s tím? Nelineární verze Kalmanova filtru !
