Łączenie filtrów Kalmana

Mój zespół buduje robota do autonomicznej nawigacji w środowisku zewnętrznym. Niedawno dostaliśmy nowy zintegrowany czujnik IMU / GPS, który najwyraźniej ma rozszerzone filtrowanie Kalmana na chipie. Daje prędkość, przechylenie i odchylenie, północ, wschód i dół, a także szerokość i długość geograficzną.

Mamy jednak również enkodery przymocowane do naszych kół, które zapewniają prędkości liniowe i kątowe. Zanim otrzymaliśmy ten nowy czujnik IMU / GPS, stworzyliśmy własny EKF do oszacowania naszego stanu za pomocą enkoderów i niektórych innych tanich czujników. Chcemy zastosować filtr wbudowany w nowy czujnik, ale także włączyć nasze miksery do miksu.

Czy jest jakiś problem z łańcuchem filtrów? Chodzi mi o to, że wykorzystalibyśmy wyjście wbudowanego EKF czujnika IMU / GPS jako aktualizacji naszego własnego EKF, podobnie jak wykorzystujemy dane odczytane z enkoderów jako aktualizację naszego EKF. Wydaje mi się to rozsądne, ale zastanawiałem się, co zwykle należy zrobić w tym przypadku.

Możesz użyć INS / GPS jako aktualizacji danych wyjściowych pierwszego EKF. W rzeczywistości nie jest to łączenie łańcuchowe, ale po prostu warunkowanie oszacowania na podstawie dodanych informacji z INS / GPS.

Załóżmy, że mamy następujące funkcje:

$x_{t+1|t}$, $P_{t+1|t}$ = EKF_PREDICT ($x_t$, $P_t$, $u_t$), dla danych wejściowych jako stan $x$, kowariancja $P$i kontrolne dane wejściowe (szacowane na podstawie odometrii) $u_t$.

i

$x_{t+1|t+1}$, $P_{t+1|t+1}$ = EKF_UPDATE ($x_{t+1|t}$, $P_{t+1|t}$, $\hat{x}_{t+1}$).

Szacunki z czujników to $\hat{x}_{t+1}$. Mamy takie rzeczy jak:

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

itp. dla wszystkich innych sposobów szacowania stanu robota. Zatem uruchomienie funkcji EKF_UPDATE dla wszystkich tych czujników jest wystarczające.

Twoja pętla będzie wyglądać mniej więcej tak:

za wszystkie czasy $t$

• Pozwolić $u_t$ być bieżącym szacunkiem odometrii / kinematyki pozy, oraz $R_u$ być hałasem w tym oszacowaniu.

• $x_{t+1|t}$, $P_{t+1|t}$ = EKF_PREDICT ($x_t$, $P_t$, $u_t$, $R_u$)

• dla wszystkich czujników $S$,

  • Pozwolić $\hat{x}^{S}_{t+1}$ być oszacowaniem pozy z tego czujnika, i $R_{S}$ być hałasem w tym oszacowaniu

  • $x_{t+1|t+1}$, $P_{t+1|t+1}$ = EKF_UPDATE ($x_{t+1|t}$, $P_{t+1|t}$, $\hat{x}_{t+1}, R_S$).

  • koniec

• koniec

Niektóre zastrzeżenia:

• Ponieważ używamy EKF, nie ma gwarancji, że kosztorys jest niezależny od kolejności aktualizacji. Oznacza to, że jeśli wykonasz INS, a następnie GPS, wynikowe oszacowanie może być inne niż w przypadku aktualizacji za pomocą GPS, a następnie INS. Zwykle nie jest to wielka sprawa, ale filtr wymaga znacznie więcej strojenia.

• Należy pamiętać, że system INS ma tendencyjność i dryf, który może mieć wpływ na długoterminową niezawodność. GPS może ci tutaj bardzo pomóc . Większość literatury jednocześnie szacuje tendencyjność i dryf w INS.