1300740014

2. Metody odtwarzania prędkości 18

oraz ich kowariancje

Pk = ^El^ek^et^Th    (2-21)

P,. = E[e_kel],    (2.22)

Estymata a posteriori jest liniową kombinacją estymaty a priori x_k i ważonej różnicy IX)iniędzy aktualnym pomiarem z_k i przewidywanym pomiarem Hx_k

x_k = x_k + K_k{z_k - Hx_k).    (2.23)

Różnicę z_k — Hx_k nazywamy innowacją pomiaru. Macierz K_k, zwana wzmocnieniem Kahnana, minimalizuje kowariancję błędu a posteriori (2.22) i dana jest wzorem

K_t    P;H^T(HP;H^T + R)-\    (2.24)

Z powyższego wynika, że filtr Kalmana przybliża przebieg procesu wykorzystując formę sprzężenia zwrotnego w postaci zaszumionego pomiaru. Równania filtru możemy podzielić na równania aktualizacji czasu, z których obliczamy wielkości a priori:

x_k    Ax_k_ i + Bw_fc_ i,    (2.25)

P, -- AP_k-iA^r + Q    (2.26)

oraz równania aktualizacji pomiaru, na podstawie których otrzymujemy wielkości a posteriori:

= *4 + K_k(z_t - Hx_k),    (2.27)

P_k=(I - K_kH)P_k,    (2.28)

gdzie K_k dane jest zależnością (2.24).

Jeżeli obiekt jest nieliniowy, to należy go zlinearyzować wokół aktualnej średniej i kowariancji. Otrzymujemy' w ten sposób rozszerzony filtr Kalmana (Extended Kalman Filter, EKF). Algorytm rozszerzonego filtru Kahnana ma również zastosowanie do jednoczesnej estymacji zmiennych stanu i parametrów obiektu [26]. W takim bowiem przypadku nawet obiekt liniowy pod względem związków między zmiennymi stanu, zazwyczaj generuje zagadnienie nieliniowe z uwagi na związki występujące między zmiennymi stanu i odtwarzanymi parametrami. Dla obiektu opisanego układem stochastycznych równań nieliniowych

fas* = /(**_!, tin-j.ton)    ,₂₂₉,

1    = h(x_t, v_k)    '    '

dokonujemy linearyzacji wokół

(2.30)

fik = /(*»-i,«*»-i,0)

\ i_k = h(x_k, 0)