===== Kálmánův filtr =====

Kálmánův filtr je algoritmus pro filtraci signálů v časové oblasti, velmi často používaný pro predikci neznámých proměnných z dat zatížených šumem a nepřesnostmi. Využívá k tomu nejen naposledy naměřených dat a model systému, ale také vektor údajů o předchozím stavu systému. 

Kálmánův filtr je nástrojem k řešení tzv. lineárně-kvadratického gaussovského problému teorie odhadu, který spočívá v odhadu stavu lineárního dynamického systému s gaussovskými náhodnými procesy při použití kvadratické ztrátové funkce. Tento problém zahrnuje tři úlohy:
  - **Predikování** spočívá v odhadu stavu systému v určitém čase za využití naměřených pozorování z časů předcházejících času odhadu ($𝑡_{𝑝𝑜𝑧𝑜𝑟𝑜𝑣á𝑛í} < 𝑡_{𝑜𝑑ℎ𝑎𝑑}$).
  - **Filtrování** spočívá v odhadu stavu systému v určitém čase za využití naměřených pozorování z tohoto času a z časů předcházejících ($𝑡_{𝑝𝑜𝑧𝑜𝑟𝑜𝑣á𝑛í} ≤ 𝑡_{𝑜𝑑ℎ𝑎𝑑}$).
  - **Vyrovnávání** spočívá v odhadu stavu systému v určitém čase za využití naměřených pozorování z časů následujících po čase odhadu ($𝑡_{𝑝𝑜𝑧𝑜𝑟𝑜𝑣á𝑛í} > 𝑡_{𝑜𝑑ℎ𝑎𝑑}$).

==== Implementace algoritmu ====

Algoritmus pracuje ve dvou krocích. V kroku predikce vytváří filtr odhad stavovu systému a nejistoy měření - variance. Jakmile je pozorován výsledek dalšího měření (nutně poškozený určitou mírou chyb, včetně náhodného šumu), jsou tyto odhady aktualizovány pomocí vážený průměru, přičemž větší váha mají odhady s menší variancí. Algoritmus je rekurzivní, může pracovat v reálné čase, s použitím aktuálního měření a dříve vypočítaného stavu a jeho variance.

Optimalita Kalmanova filtru předpokládá, že systém je Gaussovský, v němž platí pro hustotu pravděpodobnosti 

$P(x)=\frac{1}{\sigma\sqrt{2\pi}} e^{-(x-\mu)^2}$

V programovacím jazyce Python bychom mohli distribuci popsat třeba následující funkcí. Argumenty funkce jsou $\mu$ a $\sigma^2$.

<code python>
from math import *
import matplotlib.pyplot as plt
import numpy as np

def f(mu, sigma2, x):
    coefficient = 1.0 / sqrt(2.0 * pi *sigma2)
    exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)
    return coefficient * exponential
</code>

Jak provést krok aktualizace? Předpokládejme, že dvě různá měření popsaná Gaussovým rozdělením poskytují různou střední hodnotu a různou varianci. Kombinací obou měření lze vypočítat nové podmíněné rozdělení:  

<code python>
def update(mean1, var1, mean2, var2):
    new_mean = (var2*mean1 + var1*mean2)/(var2+var1)
    new_var = 1/(1/var2 + 1/var1)
    
    return [new_mean, new_var]
</code>

Predikce pak využívá výsledek kroku aktualizace v výpočtu nových hodnot:

<code python>
def predict(mean1, var1, mean2, var2):
    new_mean = mean1 + mean2
    new_var = var1 + var2
    
    return [new_mean, new_var]
</code>

Představme si nyní úlohu, ve které měříme v diskrétních časových úsecích vzdálednosti a můžeme tedy odhadnout posuny. 

<code python>
measurements = [5., 6., 7., 9., 10.]
motions =      [1., 1., 2., 1., 1.]
</code>

S počátečním odhadem ''5'' budou další odhady bezvadné, ovšem v našem experimentu nastavíme počáteční hodnotu na ''0'' s vysokou nejistotou ''10000''. Nejistoty měření vzdálednosti a posunu budou pak konstatní. 

<code python>
measurement_sig = 4.
motion_sig = 2.
mu = 0.
sig = 10000.

for n in range(len(measurements)):
    # measurement update, with uncertainty
    mu, sig = update(mu, sig, measurements[n], measurement_sig)
    print('Update: [{}, {}]'.format(mu, sig))
    # motion update, with uncertainty
    mu, sig = predict(mu, sig, motions[n], motion_sig)
    print('Predict: [{}, {}]'.format(mu, sig))

# tisk tabulky s výsledky    
print('\n')
print('Final result: [{}, {}]'.format(mu, sig))
</code>

Další čtení? [[https://ozzmaker.com/berryimu-python-code-update/]]