Filtr Kalmana - model stanowy dla pomiaru żyroskop + magnetometr

[simba92]

Witam!

Chcę mierzyć orientacje robota w przestrzeni poziomej xy. Czy ktoś mógłby mi podpowiedzieć jak ułożyć równania stanu modelu dla połączenia odczytów z żyroskopu i magnetometru z wykorzystaniem filtru kalmana ??? Chodzi mi tylko o model stanowy.

[GAndaLF]

Ja bym to zrobił tak:

ax - kąt w osi x obliczony na bazie odczytów z magnetometru

wx - prędkość kątowa w osi x z żyroskopu

dt - okres próbkowania

Równania stanu

ax(t + 1) = ax(t) + wx(t)*dt

yx(t) = ax(t)

ax jest twoją zmienną stanu, wx jest twoim pobudzeniem. Dostajesz wtedy macierze:

A = 1

B = dt

C = 1

[simba92]

GAndaLF trochę zmieniłem model stanowy,

Równania stanu:

theta(t) = theta(t-1) + w_g(t)*dt - g(t-1)*dt

g(t) = g(t-1)

;gdzie

theta() - położenie kątowe w osi Z na płaszczyźnie xy

w_g() - rzeczywista wartość prędkości kątowej mierzona przez żyroskop (bez dryfu)

g() - wartość dryfu

Stąd:

A=[1 -dt;0 1]

B=[dt;0]

C=[1 0]

Implementacja na stm32 - działa tylko....

Mam przekłamania, theta() aż o 15 stopni czyli filtr jest nie dostrojony. Bez filtru czujnik magnetometru ma przekłamania około 8 stopni (nie uwzględniłem jeszcze deklinacji około 4 stopnie w zależności od szerokości geograficznej). Magnetometr jest kalibrowany i wyznaczane są wartości offsetu dla osi X i Y, co poprawia odczyty.

Gdzieś znalazłem artykuł dot. strojenia filtru kalmana w odniesieniu do macierzy szumu pomiarowego i procesowego, które tu wpisałem na twardo z Twojego artykułu dot. filtru Kalmana akcelerometr + żyroskop, ale nie mogę tego teraz nigdzie znaleźć ??? mógłbyś mi pomóc i powiedzieć jak wyznaczyć te macierze zakłóceń ??? obliczać tylko te odchylenia standardowe dla czujnika i tyle ???

[GAndaLF]

Właśnie w wyznaczaniu tych macierzy V - szumu modelu stanowego - i W - szumu wyjścia - jest cała zabawa. Musisz to zrobić doświadczalnie sprawdzając różne wartości w symulacjach. Przepisanie na sztywno wartości z neta prawie nigdy nie da dobrych rezultatów.