Filtr Kalmana - implementacja, parametry i strojenie

Przede wszystkim po raz pierwszy próbuję stworzyć filtr Kalmana.

Wcześniej opublikowałem następujące pytanie: Odfiltruj hałas i odchylenia od wartości prędkości na StackOverflow, które opisują tło tego postu. Jest to typowa próbka wartości, które próbuję odfiltrować. Nie muszą się zmniejszać, co ma miejsce w tym przypadku. Ale tempo zmian jest zwykle takie

X ------- Y
16 --- 233,75
24 --- 234,01
26 --- 234,33
32 --- 234,12
36
--- 233,85 39 --- 233,42
47 --- 233,69
52 --- 233,68
55 --- 233,76
60 --- 232,97
66 --- 233,31
72 --- 233,99

Zaimplementowałem mój filtr Kalmana zgodnie z tym samouczkiem: Filtr Kalmana dla manekinów .

Moja implementacja wygląda następująco (pseudokod).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Zaczynam od wcześniejszej oceny 0. To wydaje się działać dobrze. Ale zauważam, że kalmanGain będzie się zmniejszał za każdym razem, gdy ta aktualizacja zostanie uruchomiona, co oznacza, że ​​ufam moim nowym wartościom, im dłużej mój filtr działa (?). Nie chce tego

Przeszedłem od używania średniej ruchomej (prostej i ważonej wykładniczo) do korzystania z tego. W tej chwili nie mogę nawet uzyskać tak dobrych rezultatów.

Moje pytanie brzmi, czy jest to właściwa implementacja i czy moja poprzednia wariancja błędu i odchylenie standardowe wyglądają dobrze zgodnie z wartościami próbki, które opublikowałem? Moje parametry są właśnie wybierane losowo, aby sprawdzić, czy mogę uzyskać dobre wyniki. Próbowałem kilku różnych zakresów, ale ze słabymi wynikami. Jeśli masz jakieś sugestie dotyczące zmian, które mogę wprowadzić, byłoby to bardzo mile widziane. Przepraszam, jeśli brakuje jakichś oczywistych rzeczy. Po raz pierwszy też tutaj zamieszczam posty.

Filtry Kalmana są przydatne, gdy sygnał wejściowy składa się z hałaśliwych obserwacji stanu pewnego liniowego układu dynamicznego. Biorąc pod uwagę szereg obserwacji stanu systemu, filtr Kalmana ma na celu rekurencyjne dostarczanie coraz lepszych oszacowań stanu systemu podstawowego. Aby zastosować go z powodzeniem, musisz mieć model dynamiki systemu, którego stan szacujesz. Jak opisano szczegółowo w Wikipedii , model ten opisuje, w jaki sposób oczekuje się zmiany stanu systemu leżącego u podstaw jednego kroku czasowego, biorąc pod uwagę jego poprzedni stan, wszelkie dane wejściowe do systemu oraz rozkładany przez Gaussa komponent stochastyczny zwany szumem procesowym.

Biorąc to pod uwagę, z twojego pytania nie wynika jasno, czy masz taki bazowy model. Połączony post wskazał, że konsumujesz wartości prędkości z czujnika. Mogą być one modelowane jako bezpośrednie obserwacje stanu systemu (gdzie stan jest jego prędkością) lub pośrednie obserwacje jego stanu (na przykład, gdzie stan jest jego pozycją). Ale, aby użyć frameworka Kalmana, musisz wybrać model ewolucji tego stanu w miarę upływu czasu; te dodatkowe informacje są wykorzystywane w celu wygenerowania optymalnego oszacowania. Filtr Kalmana nie jest magiczną czarną skrzynką, która po prostu „wyczyści” przyłożony do niego sygnał.

Powiedziawszy to, zjawisko, o którym wspomniałeś, w którym filtr Kalmana będzie coraz bardziej pewny własnej produkcji do momentu, w którym obserwacje na wejściu będą stopniowo ignorowane, zdarza się w praktyce. Można to złagodzić poprzez ręczne zwiększenie wartości w macierzy kowariancji szumu procesowego. Następnie jakościowo model przejścia stanu w systemie zawiera większy komponent stochastyczny, więc zdolność estymatora do dokładnego przewidywania następnego stanu przy obecnym stanie jest zmniejszona. Zmniejszy to jego zależność od aktualnego oszacowania stanu systemu i zwiększy jego zależność od kolejnych obserwacji, zapobiegając efektowi „ignorowania danych wejściowych”.

Jeśli dobrze to zrozumiałem, masz coś w ruchu i możesz obserwować prędkość, a ta prędkość jest głośna. Na podstawie pomiarów obserwujesz 2 rodzaje odmian. \

1. Wariacje spowodowane hałasem
2. Wariacje, ponieważ obiekt naprawdę zmienia prędkość (np. Skręt)

Powodem dla którego zyskujesz Kalmana do zera jest to, że domyślnie założyłeś, że prędkość obiektu jest stała i wszystko, co musisz zrobić, to oszacować tę prawdziwą prędkość.

„ Hej, mam obiekt poruszający się ze stałą prędkością i chcę ją oszacować ”

Twój model jest taki, gdzie jest prędkością w czasie a jest odpowiednim pomiarem.$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Ale twój obiekt nie porusza się w ten sposób. Szybkość się zmienia i nie wiesz, jak i kiedy to się zmieni.

Zamiast tego musisz powiedzieć:

„ Hej, mam obiekt, który porusza się z prędkością, ale nie jestem pewien, jak zmienia prędkość ”

Można to zrobić na wiele sposobów: Najprostszym sposobem jest zwiększenie niepewności stanu.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ gdzie i są uważane za biały szum.$q_k$$v_k$

Twoje równania filtra Kalmana będą wyglądać następująco:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

W twoim przypadku 0.05wartością jest kowariancja szumu obserwacyjnego, . Aby dokonać tej zmiany, wystarczy ustawić , stan kowariancji szumu stanu (niepewność w twoim stanie) na pewną stałą wartość.$Q^o$$Q^s$

W twoim kodzie niewielka modyfikacja to:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

Nie przez dodanie stateVariancelub w kodzie Państwo przyjąć, że jest zerem.$Q^s$

Ta stateVariancewartość może być dowolna. Opiera się na twojej pewności, jak bardzo prędkość się zmieni. Jeśli uważasz, że prędkość pozostanie dość stała, ustaw ją na małą liczbę.

W ten sposób zysk Kalmana nie spadnie do zera.

Potrzebujesz dynamicznego systemu, aby użyć filtra Kalmana.

sugerowałbym

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ Pomiar: $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

Zamiast więc używać jako stanów, wprowadź współczynnik ( ) jako stany$x$$a$

Myślę, że przydałoby się kilka pomysłów z klasycznej teorii sterowania, np . Sterownik PID .

Twój sygnał Y może być wartością zadaną regulatora u (t). Instalacja procesowa ma tylko 1, a y (t) będzie filtrowane na wyjściu. Wszystko, co musisz zrobić, to ustawić parametry (dostroić) P, I i D, aby uzyskać to, czego chcesz.

Wyjście y (t) będzie próbowało „podążać” za wejściem u (t), ale parametry określają sposób śledzenia.

Różnica wzmocnienia D sprawi, że twoja odpowiedź będzie wrażliwa na szybkie zmiany błędów. W twoim przypadku D powinien być mały. Nie chcesz, aby y (t) zmieniło się, jeśli u (t) zmieni się nagle.

Zintegrowane wzmocnienie „I” sprawi, że twoja odpowiedź będzie wrażliwa na nagromadzony błąd. Powinieneś tam umieścić wysoką wartość. Jeśli u (t) zmienia poziom i utrzymuje go na tym poziomie, błąd narasta, a następnie chcesz, aby y (t) zrobił to samo.

Wzmocnienie P może dać dobrą melodię. W każdym razie spróbuj zagrać z parametrami i zobacz, co otrzymujesz.

Istnieją jednak skomplikowane metody dostrajania, ale nie sądzę, aby były potrzebne.

Powodzenia.

Oto prosta i czysta implementacja Kalmana Filter, która jest zgodna z zapisami jak na stronie Wikipedii. https://github.com/zziz/kalman-filter