Úvod do Kalmanova filtru

Transkript

1 Kalmanův filtr = odhadovač stavu systému Úvod do Kalmanova filtru KF dává dohromady model systému a měření. Model systému použije tomu, aby odhadl, ja bude stav vypadat a poté stav porovná se sutečným měřením, a upraví ho ta, aby měření odpovídal Funguje jen pro lineární systémy (ale existují rozšíření pro nelineární). Model systému (lineární) x = + Ax + Bu + v y = Hx + w Rudolf Kalman x je vetor stavů o n prvcích, u je vetor vstupů o m prvcích a y je vetor výstupů o p prvcích. Matice n n, matice B rozměru n m, matice H rozměru p n v je procesní šum, terý zahrnuje poruchy, teré ovlivňují dynamiu systému, ale nejsou modelovány, a měření. Vetor v má stejný počet prvů jao vetor stavu x, vetor A je tedy rozměru w je šum (porucha) w má stejný počet prvů jao výstupní vetor y Předpolady v má Gaussovo rozložení s nulovou střední hodnotou E ( v ) = 0 a ovarianční maticí P = V w má Gaussovo rozložení s nulovou střední hodnotou E ( w ) = 0 a ovarianční maticí P = W Co chceme zjistit? Odhad stavu. Odhad stavu bude mít Gaussovo rozložení se střední hodnotou x ˆ a ovarianční maticí P

2 Ja to funguje? ˆ + x, ˆ + + x, P počáteční odhad Predice ace model P + Korece měření odhad + + měření Predice xˆ = Axˆ + Bu + P = AP A + V T + Korece xˆ = xˆ + K y P = P K H P de y = y H xˆ S = H P H + W T K = P H S T

3 Jednoduchý přílad Těleso padá z určité výšy, bez odporu vzduchu, jediná působící síla je gravitace. Máme dispozici měřá výšy, terý neměří příliš dobře. Chceme znát polohu a rychlost tělesa. Co bude ve stavu? To co chci určit, tedy výša a rychlost [ ] T Ja bude vypadat model? ( ) = g ( ) = ( ) yt yt y gt t 0 0 g y t y y t t t t 2 disrétně pro přírůste( t t 0 ) = g 2 ( ) = + ( ) ( ) y( + ) = y( ) + y ( ) vstupem je tíhové zrychlení, u = [ g] 2 x = y y, teré je pro všechny časové roy onstantní výstupem je výša (výstupem musí být veličina, terou přímo měříme) vyrobíme tedy příslušné matice y 0.5 v x+ = Ax + Bu + v = [ g] 0 y + + v 2 y y = Hx + w = [ 0] + [ w] y

4 y 0.5 v x+ = Ax + Bu + v = + [ g] + 0 y v 2 y y = Hx + w = [ 0] + [ w] y matice v a w neznáme, matice ABH,, jsou onstantní (v tomto příladu!) a index u nich není třeba Abychom mohli implementovat Kalmanův filtr, potřebujeme ještě znát matice V a W +. Co to je? Jsou to jediné parametry, teré ve sutečnosti můžeme měnit. Jedná se o ovarianční matice, naše odhady procesního šumu a šumu měření. Když bude velá matice V, znamená to, že modelu příliš nevěříme. Když bude velá matice W +, znamená to, že nevěříme měření. Konrétně jsou v těchto maticích rozptyly a ovariance příslušných šumů. Rozměry matic v našem onrétním příladu jsou: = V a W [ ] = + A můžeme již celý přílad implementovat v Matlabu

5 . Příprava % Kalmanuv filtr - padajici teleso clear all sutecna_poloha = 00; sutecna_rychlost = 0; g = ; % velmi mala gravitace A = [ ; 0 ]; % matice soustavy B = [0.5; ]; % vliv ace H = [ 0]; % matice mereni % odhady pocatecniho stavu pocatecni_poloha = 95; % odhadneme o 5 metru nize pocatecni_rychlost = ; % odhadneme nenulovou rychlost (smerem nahoru) x0 = [pocatecni_poloha; pocatecni_rychlost]; % prvotni odhad stavu P0 = [0 0; 0 0]; % pocatecni ovariancni matice u = -g; % ace je porad stejna V = [0.5 0; 0 0.5]; % odhad procesniho sum - timhle urcujeme ja doonaly model mame W = []; % odhad sumu mereni - timhle urcujeme ja moc dobry mera mame

6 2. Vlastní simulace + výpočet Kalmanova filtru pocet_iteraci = 0; xhat = x0; % inicializace odhadu pocatecni hodnotou P = P0; % inicializace ovariancni matice pocatecni hodnotou for i= :pocet_iteraci % pro porovnani potrebujeme sutecne hodnoty sutecna_rychlost = sutecna_rychlost - g; sutecna_poloha = sutecna_poloha + sutecna_rychlost; % provedeme simulovane mereni sum = *randn; % sum mereni ymeasured = sutecna_poloha + sum; % predice xhat = A*xhat+B*u; % predice stavu P = A*P*A' + V; % predice ovariancni matice % orece yhat = H * xhat; % jay vypada mereni, dyz je stav taovy, jay prediujeme yvlna = ymeasured - yhat; % rozdil mezi sutecnym merenim a prediovanym S = H*P*H' + W; % measurement prediction covariance K = P*H'*inv(S); % Kalmanovo zesileni xhat = xhat + K * yvlna; % orece stavu P = P - K*S*K'; % orece ovariancni matice end % a ted zalogujeme vsechna data a do odhadu a ovariancni matice dame % nove vypoctene hodnoty xhat = xhat; P = P; log_poloha(i) = sutecna_poloha; log_rychlost(i) = sutecna_rychlost; log_odhadpolohy(i) = xhat(); log_odhadrychlosti(i) = xhat(2); log_mereni(i) = ymeasured;

7 3. Vyreslení výsledů % vyresleni figure subplot(,2,) hold on plot(log_poloha,'r'); plot(log_odhadpolohy,'b'); plot(log_mereni,''); legend('sutecnost','odhad','zmereno'); title('poloha'); subplot(,2,2) hold on plot(log_rychlost,'r'); plot(log_odhadrychlosti,'b'); legend('sutecnost','odhad'); title('rychlost');

8 Co se stane, dyž budeme věřit modelu a nebudeme věřit měření? A ja toho docílíme? V = [0.0 0; 0 0.0]; % odhad procesniho sum W = [0.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ěřáu i dyž je model v pořádu, malou důvěrou měření bude trvat poměrně dlouho, než se odhad přiblíží e sutečnosti (proč tomu ta je, i dyž model přesně odpovídá?)

9 Co se stane, dyž budeme přehnaně věřit měření? Pro lepší viditelnost je zvýšen šum na měřáu. V = [0.5 0; 0 0.5]; % odhad procesniho sum W = [ ]; % odhad sumu mereni (rade 33) sum = 0*randn; % sum mereni Výsledný odhad neuvažuje šum měření, tedy blíží se tomu, co přesně je naměřeno

10 Svělá vlastnost Kalmanova filtru fůze dat z více zdrojů Upravíme přílad ta, aby bylo možné využít dvou nezávislých měřáů výšy, jednoho lepšího (s menším rozptylem) a druhého horšího. Co se změní? Predice zůstane stejná, změní se pouze orece. Protože v modelu systému x = + Ax + Bu + v y = Hx + w se změní rozměr výstupu (teď budou dva), musí se změnit i matice H. A samozřejmě taé matice W +, terá nyní bude obsahovat rozptyly měření na hlavní diagonále. Mimo diagonálu jsou její prvy nulové (proč?) Úol, upravte implementovaný přílad na použití dvou (tří) měřáů. malá nápověda: H 0 = 0 poud první měřá bude mít rozptyl např. a druhý 2 (větší rozptyl = horší měřá), bude W + 0 = 0 2

11 A co dyž bude mít měření nenulovou střední hodnotu chyby? Ja se vypořádat s offsetem? Řešení je jednoduché, zráta přidáme offset jao další proměnnou do stavu. Úol, upravte vylepšený přílad s využitím dvou měřáů na odhad offsetu. malá nápověda, připomeňme si model systému: x + = Ax + Bu + v y = Hx + w stav se změní o offsety, tedy x = [ y y o o ] 2 tím pádem se musí změnit i všechno ostatní, onrétně především: T Ax 0 0 y y =, H o o2 0 0 = 0 0

12 Co dyž 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áladní přílad s padajícím tělesem ta, že do něj zavedeme odpor vzduchu, jao lineární funci vadrátu rychlosti, tedy 2 ( ) = + y t g cy pro tuto chvíli nebudeme řešit signum (odpor je vždy proti směru rychlosti). ve zdrojovém ódu tedy upravíme cy = 0.0; % odpor vzduchu (funce vadratu rychlosti) a pro simulační výpočet sutecna_rychlost = sutecna_rychlost - g + cy*sutecna_rychlost^2; Zbyte ponechme beze změny, tedy použijeme lineární Kalmanův filtr pro nelineární soustavu a podíváme se, co se stane.

13 Vliv nelinearity roste se zvyšující se rychlostí (aby ne, dyž je závislá na vadrátu rychlosti). Co s tím? Nelineární verze Kalmanova filtru!