Posmatramo loptu koja slobodno pada. Posmatranje vršimo pomoću kamere od nekog trenutka. Sa kamere možemo da vidimo poziciju lopte svake sekunde, ali od proizvođača znamo preciznost kamere, tj. da sa kamere određujemo tačnu poziciju lopte sa disperzijom oko 3 metra. Sa druge strane, poznat nam je fizički zakon slobodnog pada, koji nam daje poziciju lopte u odnosu na prethodni vremenski trenutak formulom \[x_t=x_{t-1}+v_{t-1}\tau - \frac{1}{2}g\tau^2,\] gde je \(\tau\) vreme proteklo od prethodnog trenutka posmatranja do sadašnjeg, a \(g\) gravitaciono ubrzanje. Uz to, menja se i brzina kretanja tokom pada po zakonu \[v_t = v_{t-1}-g\tau.\] Kako nam kamera ne daje tačan položaj loptice, vrednost položaja koju dobijemo sa kamere je tačna vrednost uz dodati šum (greška merenja). To predstavljamo jednačinom \[y_t = x_t + v_t,\] gde je \(v_t \sim N(0,3)\) dodati Gausov beli šum.
Mi želimo da što bolje ocenimo stvaran položaj lopte. Na osnovu ocene u prethodnom trenutku, koristeći se pomenutim fizičkim zakonima procenimo gde bi trebalo da se nalazi lopta. Nakon toga uzmemo podatke sa kamere i cilj nam je da uz pomoć njih poboljšamo procenu. To radi Kalmanov filter.
Kalmanov filter je optimalni rekurzivni algoritam za obradu podataka. Termin filter znači da se vrši ocena trenutnog stanja, a ne predvidjanje narednih stanja. Rekurzivan je jer koristi samo prethodnu ocenu za ocenu sadašnjeg stanja, što je velika prednost u odnosu na druge algoritme, jer nije potrebno u memoriji skladištiti sve podatke. Optimalan je u smislu da ako primenimo više različitih filtera više puta, prosečan rezultat Kalmanovog filtera će biti bolji od ostalih.
Pre nego što uđemo u detalje samog Kalmanovog filtera, moramo uvesti modele nad kojim je on dizajniran da radi. To su state space modeli (modeli u prostoru stanja) ili dinamicki linearni modeli. Osnovni pojmovi kod njih su stanje i opservacija. Model je dat jednačinama \[x_t =Fx_{t-1} + Bu_t + w_t\\ y_t = Hx_t + v_t\] gde \(x_t\) predstavlja vektor stanja sistema u trenutku \(t\), a \(y_t\) opservaciju. Veličine \(w_t\) i \(v_t\) su Gausovi beli šumovi. \(Bu_t\) predstavlja uticaj spoljnih neslučajnih promenljivih na model.
Prvu jednačinu ćemo zvati jednačinom stanja a drugu jednačinom opservacije. Pokušajmo da naš primer slobodnog pada uklopimo u ovaj model. Smatraćemo da je stanje sistema u trenutku \(t\) određeno položajem i brzinom lopte u trenutku \(t\). Jednačina stanja će biti zapravo matrični zapis navedenih fizičkih zakona: \[\begin{pmatrix}x_t\\v_t\end{pmatrix} = \begin{pmatrix}1 & \tau \\ 0 & 1\end{pmatrix}\begin{pmatrix}x_{t-1}\\v_{t-1}\end{pmatrix}-\begin{pmatrix}\frac{1}{2}\tau^2\\\tau\end{pmatrix}g\] Za jednačinu opservacije uzećemo \[y_t=H\begin{pmatrix}x_t\\v_t\end{pmatrix}+e_t\] gde je \(H=(1,0)\) jer nam kamera daje samo poziciju lopte. Uzimajući u obzir preciznost kamere, dodali smo element šuma \(e_t\sim N(0,3)\).
Kalmanov filter je skup jednačina koji omogućavaju jednostavno ažuriranje informacija u dinamičkom linearnom modelu. Kao filter, on ima za cilj da oceni trenutno stanje sistema. On to postiže koristeći samo poslednje (ocenjeno) stanje sistema, novu dobijenu opservaciju i znanje koje ima o sistemu koji modelira.
Kalmanov filter, nakon dobijene opservacije, trenutno stanje ocenjuje u dva koraka:
Prvi korak nazivamo predikcija, a drugi korekcija.
Da bismo dali jednačine Kalmanovig filtera, pretpostavimo da imamo state space model: \[\begin{align*} x_t &= Fx_{t-1} + Bu_t + w_t \\ y_t &= Hx_t + v_t , \end{align*}\]gde su \(w_t\) i \(v_t\) nekorelisani Gausovi beli šumovi sa korelacionim matricama \(Q\) i \(R\), redom. (\(Bu_t\) je deterministički ulaz pa nema disperziju). Pretpostavljamo da proces počinje normalnim vektorom \(x_{0}\) koji ima srednju vrednost \(\mu_{0}\) i kovarijacionu matricu \(\Sigma_{0}\).
Jednačine prvog koraka su: \[\begin{align*} x_{t|t-1} &= Fx_{t-1|t-1} + Bu_t \\ P_{t|t-1} &= FP_{t-1|t-1}F^\top + Q \end{align*}\] Jednačine drugog koraka su: \[\begin{align*} x_{t|t}&=x_{t|t-1} + K\varepsilon_t\\ P_{t|t}&=\left( I - KH \right)P_{t|t-1} \end{align*}\]gde je \(K = P_{t|t-1}H^\top\left(HP_{t|t-1}H^\top + R\right)^{-1}\) i tu matricu nazivamo Kalmanovim pojačanjem i ona nam određuje težinu koju dajemo opservacijama naspram predikcija.
Sada možemo da primenimo filter na naš primer sa loptom. Koristićemo implementaciju Kalmanovog filtera koju smo napisali u R-u. Sve što treba da uradimo je da mu prosledimo matrice koje određuju state space model.
n <- 40
tau <- 0.1
times <- 0:(n-1)
actual <- -4.9*tau^2*times^2
sim <- actual + sqrt(3)*rnorm(n)
X0 <- sqrt(3)*rnorm(2)
P0 <- 3*diag(2)
Fk <- matrix(c(1,0,tau,1),nrow=2,ncol=2)
Qk <- matrix(0,2,2)
Hk <- matrix(c(1,0),nrow=1)
Rk <- 3
Bkuk <- c(-4.9*tau^2, -9.8*tau)
kf <- Kalman$new(X0,P0,Fk,Qk,Hk,Rk,Bkuk)
flt <- c()
pflt <- c()
flt.std <- c()
for(i in times+1) {
step <- kf$next.step(sim[i])
upd <- step$updated
pred <- step$predicted
flt <- c(flt, upd$Xk[1])
pflt <- c(pflt, pred$Xk[1])
flt.std <- c(flt.std, sqrt(upd$Pk[1]))
}
plot.asf(actual, sim, flt, flt.std)Razmotrimo prvo \(AR(2)\) model \[ X_t = \varphi_1X_{t-1} + \varphi_2X_{t-2} + e_t \] gde je \(e_t \sim N(0,\sigma^{2})\). Vidimo da su nam za ovakav proces potrebne vrednosti \(X_{t-1}\) i \(X_{t-2}\) da bismo prognozirali proces. Stoga se lako vidi da je ovaj proces moguće opisati state space modelom: \[ \underbrace{\begin{pmatrix}X_{t}\\X_{t-1}\end{pmatrix}}_{x_t} = \underbrace{\begin{pmatrix} \varphi_1 & \varphi_2 \\ 1 & 0 \end{pmatrix}}_{F} \underbrace{\begin{pmatrix}X_{t-1}\\X_{t-2}\end{pmatrix}}_{x_{t-1}} + \underbrace{\begin{pmatrix}e_t\\0\end{pmatrix}}_{w_t} \] Za jednačinu opservacija možemo uzeti \(y_t = (1\ 0)x_t\) koja jednostavno uzme prvu komponentu vektora stanja (ovde smo pretpostavili da možemo da posmatramo (merimo) baš \(X_t\) i to bez šuma).
Ovaj rezultat se može uopštiti na proizvoljan \(AR(p)\) model, gde jednačina stanja postaje \[ \underbrace{\begin{pmatrix}X_{t}\\X_{t-1}\\X_{t-2}\\\vdots\\X_{t-p+1}\end{pmatrix}}_{x_t} = \underbrace{\begin{pmatrix} \varphi_1 & \varphi_2 & \cdots & \varphi_{p-1} & \varphi_p \\ 1 & 0 & \cdots & 0 & 0 \\ 0 & 1 & \cdots & 0 & 0 \\ \vdots & & \ddots & \vdots & \vdots \\ 0 & \cdots & 0 & 1 & 0 \\ \end{pmatrix}}_{F} \underbrace{\begin{pmatrix}X_{t-1}\\X_{t-2}\\X_{t-3}\\\vdots\\X_{t-p}\end{pmatrix}}_{x_{t-1}} + \underbrace{\begin{pmatrix}e_t\\0\\0\\\vdots\\0\end{pmatrix}}_{w_t} , \] a matrica opservacija \(H=\left(1,0,0,\ldots,0\right)\)
n <-40
phi1 <- 0.4
phi2 <- 0.3
times <- 0:(n-1)
actual <- arima.sim(list(order=c(2,0,0), ar=c(phi1, phi2)), n=n)
sim <- actual + rnorm(n)
X0 <- rnorm(2,0,3)
P0 <- 9*diag(2)
Fk <- matrix(c(phi1,1,phi2,0),nrow=2,ncol=2)
Qk <- matrix(c(1,0,0,0),nrow=2,ncol=2)
Hk <- matrix(c(1,0),nrow=1)
Rk <- 1
Bkuk <- 0
kf <- Kalman$new(X0,P0,Fk,Qk,Hk,Rk,Bkuk)
flt <- c()
pflt <- c()
flt.std <- c()
for(i in times+1) {
step <- kf$next.step(sim[i])
upd <- step$updated
pred <- step$predicted
flt <- c(flt, Hk%*%upd$Xk)
pflt <- c(pflt, Hk%*%pred$Xk)
flt.std <- c(flt.std, sqrt(upd$Pk[1]))
}
plot.asf(actual, sim, flt, flt.std)