EKF a orientace z IMU

Tohle je statická kopie kapitoly pro vyhledávače. Interaktivní verze má animované obrázky, kontrolní otázky a tlačítka, která příklad načtou do kalkulačky.

Kalmanův filtr předpokládá, že je všechno lineární. Orientace není: gravitace se do os promítá kosiny úhlů a kvaterniony se násobí. Rozšířený Kalmanův filtr to řeší tím nejprostším způsobem — v každém kroku si soustavu zlinearizuje kolem současného odhadu.

F_k = ^∂f⁄_∂x│_x̂ H_k = ^∂h⁄_∂x│_x̂Jakobiány, počítané znovu v každém kroku

Co je ve stavu — a proč zrovna tohle

Stav je kvaternion orientace a k němu bias gyroskopu, dohromady sedm čísel. Ten bias je celý důvod, proč sem chodit z komplementárního filtru: ten drift jen ohraničí, kdežto EKF ho odhadne a odečte. Když se drift mění s teplotou, jde odhad s ním.

Gyroskop pohání predikci, akcelerometr opravuje. A opravuje jen směr naměřeného vektoru, nikdy jeho velikost — protože ve velikosti sedí lineární zrychlení, tedy přesně to, čemu se věřit nesmí.

Po 24 s: EKF má náklon 0,2499 proti pravdě 0,2500, a odhadl drift 0,0327 proti skutečným 0,0200 rad/s. Samotný gyroskop je mimo o 0,228 rad.
Stáhni drift na nulu: růžové mračno akcelerometru a červená integrace se srovnají a EKF nemá co opravovat. Přidej drift a červená odjede lineárně bez konce — jenže EKF ji drží, protože si drift SÁM odhadl jako součást stavu. To je ten rozdíl proti komplementárnímu filtru, který drift neodhaduje, jen ho ohraničí.

Růžové mračno je náklon jen z akcelerometru, červená jen z integrovaného gyroskopu, fialová EKF. Stáhni drift na nulu a přidej ho zpátky — červená odjede, fialová ne.

Co akcelerometr říct nemůže

Otočením kolem svislé osy se gravitace nezmění. Z akcelerometru se tedy azimut nedá zjistit vůbec a odpovídající složka biasu zůstane neodhadnutá. Není to vada filtru, je to geometrie. Kdo chce azimut, musí přidat magnetometr — a s ním všechny jeho potíže s železem v okolí, které popisuje SensorLab.

Madgwick: totéž skoro zadarmo. Jeden gradientní krok místo kovariancí, jedna konstanta β místo matic Q a R, 109 operací na vzorek. Na nezaujatém gyroskopu je prakticky stejně dobrý. Rozdíl se ukáže tam, kde je bias — Madgwick ho neodhaduje. Testy téhle labky pouštějí oba na tentýž záznam právě proto, aby ten rozdíl byl vidět, a ne jen tvrzen.

Tři věci, na kterých EKF nejčastěji ztroskotá. Špatný Jakobián. Analytická derivace se opíše se znaménkem a filtr se rozjede pomalu a nevysvětlitelně. Jediná obrana je porovnat ji s numerickou derivací — v testech téhle labky to je. Kvaternion, který přestal být jednotkový. Po každé korekci se musí znormovat. Příliš důvěřivé R. Když se stroj rozjede, akcelerometr měří zrychlení a filtr to bere jako náklon; buď se R při pohybu zvětší, nebo se korekce vynechá.