W niniejszej serii artykułów mam zamiar zająć się filtrami Kalmana. Zacznę od niezbędnej dawki teorii, by przejść do przykładów praktycznych.
Najpierw na podstawie rzeczywistych danych pomiarowych z czujnika zaprojektujemy filtr w Matlabie, a potem przeniesiemy implementację na mikrokontroler STM32.
Do pełnego zrozumienia artykułu wskazana jest
podstawowa wiedza z teorii sterowania.
Filtr Kalmana (KF) jest obserwatorem stanu minimalizującym średniokwadratowy błąd estymacji. Oznacza to, że algorytm na podstawie pomiarów wejścia i wyjścia obiektu szacuje jego wewnętrzny stan, a oszacowanie stanu jest statystycznie optymalne.
Oryginalnie filtr Kalmana jest przeznaczony do pracy z układami liniowymi, jednak po niewielkich modyfikacjach jest możliwa również praca z układami niestacjonarnymi (których model zmienia się w czasie) i nieliniowymi. KF dla układów nieliniowych nazywany jest Rozszerzonym Filtrem Kalmana (EKF - Extended Kalman Filter).
Filtr Kalmana został wynaleziony w 1960 roku i od tego czasu znalazł zastosowanie w wielu gałęziach techniki takich jak loty w kosmos, robotyka, procesy chemiczne, a nawet meteorologia czy ekonomia.
Założenia wstępne
Rozpatrywany proces można przedstawić za pomocą dyskretnoczasowego modelu w przestrzeni stanu:
x(t) oznacza stan w chwili czasu t = {0, 1, . . . }, y(t) wyjście układu, A macierz stanu, B macierz wejścia, C macierz wyjścia. Symbole v(t) i w(t) oznaczają szum procesowy (szum tworzący) i szum pomiarowy. Są to wzajemnie niezależne realizacje białego szumu gaussowskiego o zerowej wartości oczekiwanej i znanych macierzach kowariancji V i W. Odwzorowują one niedoskonałość przyjętego modelu oraz niedokładność aparatury pomiarowej.
Stan początkowy x(0) jest realizacja gaussowskiej zmiennej losowej niezależnej od v(t) i w(t) o wartości oczekiwanej x0 i macierzy kowariancji P0:
Równania filtru Kalmana
Algorytm składa się z dwóch faz. Pierwsza nazywana jest aktualizacją czasu i polega na wyliczeniu jednokrokowej predykcji stanu czyli estymaty a priori i jej macierzy kowariancji P(t + 1|t):
Potem następuje zwiększenie czasu t ← t + 1 i algorytm przechodzi do drugiej fazy, czyli aktualizacji pomiarów:
Wartość ε(t) jest różnicą pomiędzy najnowszym pomiarem y(t), a wartością spodziewaną na podstawie aktualnego oszacowania stanu. Wartość ta nazywana jest innowacją, ponieważ przynosi nowe dane o procesie. Macierz K(t) nazywana jest wzmocnieniem Kalmana. Decyduje ona, jaki wpływ na estymatę stanu a posteriori ma nowy pomiar, a jaki estymata a priori.
Filtr Kalmana - przykład w Matlabie
To tyle jeśli chodzi o teorię. Aby pokazać działanie filtru w praktyce przygotowałem prosty przykład w Matlabie. Jest to pomiar zaszumionego napięcia.
Pomiary są wykonywane z odchyleniem standardowym 10. Przez pierwszą połowę symulacji wartość oczekiwana napięcia wynosi 0V. Następnie następuje skok wartości oczekiwanej do 100V. Dzięki temu będzie można zaobserwować jak filtr radzi sobie przy braku zmian wartości zadanej i przy skokowej zmianie o dużą wartość. Oto kod programu:
% Parametry symulacji
dt = 0.1;
t_end = 10;
t = 0:dt:t_end;
% Odchylenie standardowe pomiaru
std_dev = 10;
% Model stanowy
A = 1;
C = 1;
% Macierze kowariancji szumow
V = 5*std_dev*dt;
W = std_dev*std_dev;
% Wartosci poczatkowe
x0 = 0;
P0 = 1;
xpri = x0;
Ppri = P0;
xpost = x0;
Ppost = P0;
% Wektory wynikow
Y = zeros(1, size(t,2));
Yf = Y;
Yf(1) = x0;
for i = 1:size(t,2)
if i < 50
Y(i) = std_dev*randn(1);
else
Y(i) = std_dev*randn(1) + 100;
end
if i > 1
% aktualizacja czasu
xpri = A*xpost;
Ppri = A*Ppost*A' + V;
% aktualizacja pomiarow
eps = Y(i) - C*xpri;
S = C*Ppri*C' + W;
K = Ppri*C'*S^(-1);
xpost = xpri + K*eps;
Ppost = Ppri - K*S*K';
Yf(i) = xpost;
end
end
plot(t, Y, 'b', t, Yf, 'r', 'LineWidth', 1.5)
title('Filtr Kalmana')
xlabel('Czas')
ylabel('Sygnal mierzony')
legend('Wartosc mierzona', 'Wartosc estymowana')
Mamy do czynienia z układem o jednej zmiennej stanu, dlatego macierze A, C, V i W upraszczają się do zwykłych liczb. Podobnie ma się sytuacja z macierzami P, K i S. Wariancję szumu pomiarowego ustawiłem jako kwadrat odchylenia standardowego czujnika.
Wariancję szumu procesowego, wartość początkową oszacowania stanu oraz jego wariancję wybiera się podczas strojenia filtru. Przeanalizujemy teraz wpływ zmian tych wartości na wyniki symulacji.
Pierwszy wykres przedstawia wyniki symulacji dla V=5*stddev*dt, P_0 = 1, x0 = 0. Można zauważyć, że w początkowej fazie oszacowanie stanu waha się o około 5V od wartości zadanej. Po skokowej zmianie sygnału filtr potrzebuje około 1.5s aby nadążyć za zmianą.
V=5*stddev*dt, P0 = 1, x_0 = 0
Drugi wykres przedstawia wyniki symulacji dla zwiększonej wartości wariancji szumu procesowego. Tym razem odpowiedź na skokową zmianę jest dużo szybsza, ale dokładność filtracji w stanie ustalonym jest zdecydowanie mniejsza.
V=50*stddev*dt, P_0 = 1, x_0 = 0
Skutki zmniejszenia wartości wariancji szumu procesowego przedstawiono na poniższym wykresie. Tym razem wahania sygnału w stanie ustalonym są bardzo niewielkie kosztem zwiększonego czasu ustalania po skokowej zmianie.
V=1*stddev*dt, P_0 = 1, x_0 = 0
Kolejny wykres przedstawia wpływ dużej wartości początkowej wariancji oszacowania stanu. W początkowej fazie symulacji filtr dużo bardziej wierzy pomiarom niż aktualnemu oszacowaniu stanu, dlatego zmiana wartości jest gwałtowna mimo małej wariancji szumu procesowego.
Po kilku pierwszych iteracjach algorytmu wariancja oszacowania stanu maleje i sygnał znów wykazuje niewielkie zmiany:
V=1*stddev*dt, P_0 = 10000, x_0 = 30
Ostatni wykres przedstawia symulację dla niskiej początkowej wartości oszacowania stanu. Tym razem filtr bardziej bazuje na aktualnym oszacowaniu stanu niż na pomiarach, dlatego zmiana sygnału nie jest tak gwałtowna mimo dużej wariancji szumu procesowego.
Podobnie jak w poprzednim przypadku zmiana wartości początkowych ma znaczenie tylko w pierwszych kilku iteracjach filtru.
V=50*stddev*dt, P_0 = 0.00001, x_0 = 30
Podsumowanie
W tej części artykułu zaprezentowałem podstawy teoretyczne filtru Kalmana oraz omówiłem jego strojenie. W kolejnej części zajmę się projektowaniem filtru w Matlabie dla danych pomiarowych zebranych z rzeczywistych czujników - akcelerometru i żyroskopu.
Dołącz do 20 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY na bazie Arduino i Raspberry Pi.
To nie koniec, sprawdź również
Przeczytaj powiązane artykuły oraz aktualnie popularne wpisy lub losuj inny artykuł »
Dołącz do 20 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY z Arduino i RPi.
Trwa ładowanie komentarzy...