2019. Procjena prostorne orijentacije, ili kako se ne plašiti Mahoney i Madgwick filtera04.02.2019. Da li je sistem vektora ortogonan

44. Francuski sistem vlasti, pravo glasa i izborni sistem Francuska je mješovita (polupredsjednička) republika, sistem vlasti je zasnovan na principu podjele vlasti.

Poglavlje IV. Dual Head Compliance System. Sistem insekata. Minisistem

Poglavlje IV. Dual Head Compliance System. Sistem insekata. Mini-sistem dvostruke glave korespondentni sistem Postoje dva sistema korespondencije glave na prstima ruku i nogu: sistem "ljudskog tipa" i sistem "životinjskog tipa". Sistem "ljudskog tipa".

Prvi emocionalni centar - skeletni sistem, zglobovi, krvotok, imuni sistem, koža

Prvi emocionalni centar je skeletni sistem, zglobovi, krvotok, imuni sistem, koža Zdravo stanje organa povezanih sa prvim emocionalnim centrom zavisi od osećaja sigurnosti u ovom svetu. Ako ste uskraćeni za podršku porodice i prijatelja koju ste

Ako se na ravni odaberu bilo koja dva međusobno okomita vektora jedinične dužine (slika 7), onda se proizvoljni vektor u istoj ravni može proširiti u smjerovima ova dva vektora, tj. predstaviti ga u obliku

gdje su brojevi jednaki projekcijama vektora na smjerove osa. Pošto je projekcija na osu jednaka proizvodu dužine i kosinusa ugla sa osom, onda, podsećajući na definiciju skalarnog proizvoda , možemo pisati

Slično, ako u trodimenzionalnom prostoru izaberemo bilo koja tri međusobno okomita vektora jedinične dužine, onda se proizvoljni vektor u ovom prostoru može predstaviti kao

U Hilbertovom prostoru se mogu razmatrati i sistemi parno ortogonalnih vektora ovog prostora, tj. funkcije

Takvi sistemi funkcija nazivaju se ortogonalnim sistemima funkcija i igraju važnu ulogu u analizi. Oni se susreću u raznim problemima matematičke fizike, integralnih jednadžbi, približnih proračuna, teorije funkcija realne varijable itd. stvoriti opšti koncept Hilbertov prostor.

Hajde da damo precizne definicije. Funkcijski sistem

naziva se ortogonalnim ako su bilo koje dvije funkcije ovog sistema ortogonalne jedna na drugu, tj.

U trodimenzionalnom prostoru, tražili smo da dužine vektora sistema budu jednake jedan. Podsjećajući na definiciju dužine vektora, vidimo da se u slučaju Hilbertovog prostora ovaj zahtjev piše na sljedeći način:

Sistem funkcija koji zadovoljava zahtjeve (13) i (14) naziva se ortogonalnim i normaliziranim.

Navedimo primjere takvih sistema funkcija.

1. Na intervalu razmotrite slijed funkcija

Sve dvije funkcije iz ovog niza su ortogonalne jedna prema drugoj. Ovo se potvrđuje jednostavnim proračunom odgovarajućih integrala. Kvadrat dužine vektora u Hilbertovom prostoru je integral kvadrata funkcije. Dakle, kvadrati dužina vektora sekvence

suština integrala

tj. naš vektorski niz je ortogonan, ali nije normalizovan. Dužina prvog vektora niza je i sve

ostali imaju dužinu. Deljenjem svakog vektora njegovom dužinom dobijamo ortogonalni i normalizovani sistem trigonometrijske funkcije

Ovaj sistem je istorijski jedan od prvih i najvažnijih primera ortogonalnih sistema. Nastala je u radovima Eulera, D. Bernoullija, D'Alemberta u vezi s problemom vibracija struna. Njegovo proučavanje odigralo je ključnu ulogu u razvoju cjelokupne analize.

Pojava ortogonalnog sistema trigonometrijskih funkcija u vezi sa problemom vibracija struna nije slučajna. Svaki problem malih oscilacija sredine dovodi do određenog sistema ortogonalnih funkcija koje opisuju takozvane prirodne oscilacije datog sistema (vidi § 4). Na primjer, u vezi s problemom vibracija sfere pojavljuju se takozvane sferne funkcije, u vezi s problemom vibracija kružne membrane ili cilindra pojavljuju se tzv. cilindrične funkcije itd.

2. Možemo dati primjer ortogonalnog sistema funkcija čija je svaka funkcija polinom. Takav primjer je niz Legendreovih polinoma

tj. postoji (do konstantnog faktora) izvod reda od . Zapisujemo prvih nekoliko polinoma ovog niza:

Očigledno, postoji polinom stepena uopšte. Ostavljamo čitaocu da sam provjeri da li su ovi polinomi ortogonalni niz na intervalu

opšta teorija ortogonalne polinome (tzv. ponderisane ortogonalne polinome) razvio je izvanredni ruski matematičar P. L. Čebišev u drugoj polovini 19. veka.

Ekspanzija u ortogonalnim sistemima funkcija. Kao iu trodimenzionalnom prostoru, svaki vektor se može predstaviti

kao linearna kombinacija tri uparna ortogonalna vektora jedinične dužine

u prostoru funkcija javlja se problem proširenja proizvoljne funkcije u niz u terminima ortogonalnog i normaliziranog sistema funkcija, tj. predstavljanja funkcije u obliku

U ovom slučaju, konvergencija niza (15) funkciji se razumijeva u smislu udaljenosti između elemenata u Hilbertovom prostoru. To znači da srednja kvadratna devijacija parcijalne sume serije od funkcije teži nuli na , tj.

Ova konvergencija se obično naziva "prosječna konvergencija".

Proširenja u različitim sistemima ortogonalnih funkcija često se susreću u analizi i važna su metoda za rješavanje problema matematičke fizike. Tako, na primjer, ako je ortogonalni sistem sistem trigonometrijskih funkcija na intervalu

onda je takvo proširenje klasično proširenje funkcije u trigonometrijski niz

Pretpostavimo da je proširenje (15) moguće za bilo koju funkciju iz Hilbertovog prostora i pronađite koeficijente takve ekspanzije. Da bismo to učinili, množimo obje strane jednakosti skalarno istom funkcijom našeg sistema. Dobijamo jednakost

od čega je, zbog činjenice da je at određena vrijednošću koeficijenta

Vidimo da su, kao iu običnom trodimenzionalnom prostoru (vidi početak ovog paragrafa), koeficijenti jednaki projekcijama vektora na smjerove vektora .

Podsjećajući na definiciju skalarnog proizvoda, dobijamo da su koeficijenti proširenja funkcije u terminima ortogonalnog i normaliziranog sistema funkcija

određuju se formulama

Kao primjer, razmotrite ortogonalni normalizirani trigonometrijski sistem funkcija dat gore:

Dobili smo formulu za izračunavanje koeficijenata proširenja funkcije u trigonometrijski niz, uz pretpostavku, naravno, da je to proširenje moguće.

Ustanovili smo oblik koeficijenata proširenja (18) funkcije u terminima ortogonalnog sistema funkcija pod pretpostavkom da se takva ekspanzija odvija. Međutim, beskonačan ortogonalni sistem funkcija može se pokazati nedovoljnim za proširenje bilo koje funkcije iz Hilbertovog prostora u smislu njega. Da bi takva dekompozicija bila moguća, sistem ortogonalnih funkcija mora zadovoljiti dodatni uslov- takozvani uslov potpunosti.

Ortogonalni sistem funkcija se naziva kompletnom ako joj je nemoguće dodati jednu funkciju koja nije identično nula i ortogonalna svim funkcijama sistema.

Lako je dati primjer nekompletnog ortogonalnog sistema. Da bismo to učinili, uzimamo neki ortogonalni sistem, na primjer, isti

sistem trigonometrijskih funkcija i isključiti jednu od funkcija ovog sistema, na primjer, preostali beskonačni sistem funkcija

će i dalje biti ortogonalna, naravno, neće biti potpuna, budući da je funkcija : koju smo isključili ortogonalna na sve funkcije sistema.

Ako sistem funkcija nije potpun, onda se svaka funkcija iz Hilbertovog prostora ne može proširiti u smislu njega. Zaista, ako pokušamo da proširimo nultu funkciju ortogonalnu na sve funkcije sistema u takvom sistemu, tada će, na osnovu formule (18), svi koeficijenti biti jednaki nuli, dok funkcija nije jednaka nuli.

Vrijedi sljedeća teorema: ako je dat kompletan ortogonalni i normalizirani sistem funkcija u Hilbertovom prostoru, onda se svaka funkcija može proširiti u niz u smislu funkcija ovog sistema

U ovom slučaju, koeficijenti proširenja jednaki su projekcijama vektora na elemente ortogonalnog normaliziranog sistema

Pitagorina teorema u § 2 u Hilbertovom prostoru omogućava nam da pronađemo zanimljivu relaciju između koeficijenata i funkcije.Označimo razlikom između i zbirom prvih članova njenog niza, tj.

Takav podskup vektora $\backslash levo\backslash (\; \backslash varphi\_i\; \backslash desno\backslash )\backslash podskup\; H$ da su bilo koja različita dva od njih ortogonalna, odnosno da je njihov proizvod tačaka nula:

$(\backslash varphi\_i,\; \backslash varphi\_j)\; =\; 0$.

Ortogonalni sistem, ako je potpun, može se koristiti kao osnova za prostor. U ovom slučaju, dekompozicija bilo kojeg elementa $\backslash vec\; a$ može se izračunati pomoću formula: $\backslash vec\; a\; =\; \backslash sum\_(k)\; \backslash alpha\_i\; \backslash varphi\_i$, Gdje $\backslash alpha\_i\; =\; \backslash frac((\backslash vec\; a,\; \backslash varphi\_i))((\backslash varphi\_i,\; \backslash varphi\_i))$.

Slučaj kada je norma svih elemenata $||\backslash varphi\_i||=1$, naziva se ortonormalnim sistemom.

Ortogonalizacija

Svaki potpuni linearno nezavisan sistem u konačnodimenzionalnom prostoru je osnova. Od proste baze, dakle, može se preći na ortonormalnu bazu.

Ortogonalna dekompozicija

Kada se dekomponuju vektori vektorskog prostora u ortonormalnoj bazi, izračun skalarnog proizvoda je pojednostavljen: $(\backslash vec\; a,\; \backslash vec\; b)\; =\; \backslash sum\_(k)\; \backslash alpha\_k\backslash beta\_k$, Gdje $\backslash vec\; a\; =\; \backslash sum\_(k)\; \backslash alpha\_k\; \backslash varphi\_k$ I $\backslash vec\; b\; =\; \backslash sum\_(k)\; \backslash beta\_k\; \backslash varphi\_k$.

vidi takođe

Napišite recenziju na članak "Ortogonalni sistem"

Izvod koji karakteriše ortogonalni sistem

Definicija. Vektoria Ib nazivaju ortogonalnim (okomitim) jedni prema drugima ako su njihovi skalarni proizvod jednako nuli, tj.a × b = 0.

Za vektore koji nisu nula a I b nulti skalarni proizvod znači da je cos j= 0, tj. . Nulti vektor je ortogonan na bilo koji vektor, jer a × 0 = 0.

Vježbajte. Neka i biti ortogonalni vektori. Tada je prirodno uzeti u obzir dijagonalu pravokutnika sa stranama i . Dokaži to

one. kvadrat dužine dijagonale pravokutnika jednak je zbroju kvadrata dužina njegove dvije neparalelne stranice(Pitagorina teorema).

Definicija. Vektorski sistema 1 ,…, a m se naziva ortogonalnim ako su bilo koja dva vektora ovog sistema ortogonalna.

Dakle, za ortogonalni sistem vektora a 1 ,…,a m jednakost je istinita: a i × a j= 0 at i¹ j, i= 1,…, m; j= 1,…,m.

Teorema 1.5. Ortogonalni sistem koji se sastoji od vektora koji nisu nula je linearno nezavisan. .

□ Dokažimo kontradikcijom. Pretpostavimo da je ortogonalni sistem vektora koji nisu nula a 1 , …, a m linearno zavisna. Onda

l 1 a 1 + …+ l ma m= 0 , pri čemu . (1.15)

Neka je, na primjer, l 1 ¹ 0. Pomnožite sa a 1 obje strane jednakosti (1.15):

l 1 a 1× a 1 + …+ l m a m × a 1 = 0.

Svi članovi, osim prvog, jednaki su nuli zbog ortogonalnosti sistema a 1 , …, a m. Onda l 1 a 1× a 1 =0, odakle slijedi a 1 = 0 , što je u suprotnosti sa uslovom. Naša pretpostavka se pokazala pogrešnom. Dakle, ortogonalni sistem vektora koji nisu nula je linearno nezavisan. ■

Vrijedi sljedeća teorema.

Teorema 1.6. U prostoru R n uvijek postoji baza koja se sastoji od ortogonalnih vektora (ortogonalna baza)
(bez dokaza).

Ortogonalne baze su zgodne, prije svega, jer se koeficijenti ekspanzije proizvoljnog vektora u takvim bazama lako određuju.

Neka je potrebno pronaći dekompoziciju proizvoljnog vektora b u ortogonalnoj osnovi e 1 ,…,e n. Sastavimo ekspanziju ovog vektora sa do sada nepoznatim koeficijentima ekspanzije u ovoj bazi:

Pomnožite obje strane ove jednakosti skalarno vektorom e 1 . Na osnovu aksioma 2° i 3° skalarnog proizvoda vektora, dobijamo

Pošto su bazni vektori e 1 ,…,e n su međusobno ortogonalni, tada su svi skalarni proizvodi baznih vektora, osim prvog, jednaki nuli, tj. koeficijent se određuje po formuli

Množenjem jednakosti (1.16) drugim baznim vektorima, dobijamo jednostavne formule za izračunavanje koeficijenata proširenja vektora b :

Formule (1.17) imaju smisla jer .

Definicija. Vectora naziva se normaliziranim (ili jediničnim) ako je njegova dužina jednaka 1, tj. (a , a )= 1.

Bilo koji vektor različit od nule može se normalizirati. Neka a ¹ 0 . Zatim , i vektor je normalizirani vektor.

Definicija. Vektorski sistem e 1 ,…,e n se naziva ortonormalno ako je ortogonalno i ako je dužina svakog vektora sistema 1, tj.

Kako prostor R n uvijek ima ortogonalnu bazu i vektori te baze mogu biti normalizirani, onda R n uvijek ima ortonormalnu bazu.

Primjer ortonormalne baze za prostor Rn je sistem vektora e 1 ,=(1,0,…,0),…, e n=(0,0,…,1) sa skalarnim proizvodom definisanim jednakošću (1.9). U ortonormalnoj osnovi e 1 ,=(1,0,…,0),…, e n=(0,0,…,1) formule (1.17) za određivanje koordinata dekompozicije vektora b imaju najjednostavniji oblik:

Neka a I b su dva proizvoljna vektora u prostoru Rn sa ortonormalnom osnovom e 1 ,=(1,0,…,0),…, e n=(0,0,…,1). Označite koordinate vektora a I b u osnovi e 1 ,…,e n odnosno kroz a 1 ,…,a n I b 1 ,…, b n i pronađite izraz za skalarni proizvod ovih vektora u smislu njihovih koordinata u ovu osnovu, tj. Pretvarajmo se to

Iz posljednje jednakosti, na osnovu aksioma skalarnog proizvoda i relacija (1.18), dobijamo

Konačno imamo

dakle, u ortonormalnoj bazi, skalarni proizvod bilo koja dva vektora jednak je zbroju proizvoda odgovarajućih koordinata ovih vektora.

Razmotrimo sada potpuno proizvoljnu (općenito govoreći, ne ortonormalnu) bazu u n-dimenzionalnom euklidskom prostoru R n i pronađemo izraz za skalarni proizvod dva proizvoljna vektora a I b kroz koordinate ovih vektora u specificiranoj osnovi. f 1 ,…,f n Euklidski prostor R n skalarni proizvod bilo koja dva vektora bio je jednak zbroju proizvoda odgovarajućih koordinata ovih vektora, potrebno je i dovoljno da osnova f 1 ,…,f n bio ortonormalan.

Zaista, izraz (1.20) postaje (1.19) ako i samo ako su zadovoljeni odnosi koji uspostavljaju ortonormalnost baze f 1 ,…,f n.

O čemu pričamo

Pojava na Habréu posta o Madgwick filteru je na svoj način bila simboličan događaj. Očigledno je opšte interesovanje za dronove oživelo interesovanje za problem procene orijentacije tela na osnovu inercijalnih merenja. Istovremeno, tradicionalne metode zasnovane na Kalmanovom filteru prestale su da zadovoljavaju javnost – bilo zbog visokih zahtjeva za računskim resursima koji su neprihvatljivi za dronove, bilo zbog složenih i neintuitivnih postavki parametara.

Objavu je pratila vrlo kompaktna i efikasna implementacija filtera u C-u. Međutim, sudeći po komentarima, fizičko značenje ovog koda, kao i cijelog članka, nekome je ostalo nejasno. Pa, budimo iskreni: Madgwick filter je najzamršeniji u grupi filtera koji se općenito temelji na vrlo jednostavnim i elegantnim principima. O ovim principima će se raspravljati u mom postu. Ovdje neće biti koda. Moj post nije priča o nekoj specifičnoj implementaciji algoritma procjene orijentacije, već poziv da izmislite vlastite varijacije na datu temu, kojih može biti mnogo.

Orijentaciono predstavljanje

Prisjetimo se osnova. Da bi se procijenila orijentacija tijela u prostoru, potrebno je prvo odabrati neke parametre koji zajedno jedinstveno određuju ovu orijentaciju, tj. zapravo, orijentacija pridruženog koordinatnog sistema u odnosu na uslovno stacionarni sistem - na primjer, NED (sjever, istok, dolje) geografski sistem. Zatim treba napraviti kinematičke jednačine, tj. izraziti brzinu promjene ovih parametara u smislu ugaone brzine iz žiroskopa. Konačno, vektorska mjerenja sa akcelerometara, magnetometara itd. moraju biti uključena u proračun. Evo najčešćih načina za predstavljanje orijentacije:

Eulerovi uglovi- roll (roll, ), pitch (pitch, ), heading (heading, ). Ovo je najjasniji i najsažetiji skup orijentacijskih parametara: broj parametara je tačno jednak broju rotacijskih stupnjeva slobode. Za ove uglove možemo pisati kinematičke Eulerove jednačine. Vrlo su popularni u teorijskoj mehanici, ali su od male koristi u problemima navigacije. Prvo, poznavanje uglova ne dozvoljava vam da direktno konvertujete komponente bilo kog vektora iz vezanog u geografski koordinatni sistem ili obrnuto. Drugo, pri nagibu od ±90 stepeni, kinematičke jednačine se degenerišu, kotrljanje i pravac postaju neizvesni.

Matrica rotacije je matrica 3x3 kojom se pomnoži bilo koji vektor u pridruženom koordinatnom sistemu da bi se dobio isti vektor u geografskom sistemu: . Matrica je uvijek ortogonalna, tj. . Kinematska jednadžba za to ima oblik .
Evo matrice komponenti ugaone brzine mjerenih žiroskopima u spregnutom koordinatnom sistemu:

Matrica rotacije je nešto manje vizualna od Eulerovih uglova, ali za razliku od njih, omogućava vam direktnu transformaciju vektora i ne gubi značenje za bilo koju kutnu poziciju. Sa računske tačke gledišta, njegov glavni nedostatak je redundantnost: radi tri stepena slobode, uvodi se devet parametara odjednom, i svi se moraju ažurirati prema kinematičkoj jednačini. Problem se može malo pojednostaviti korištenjem ortogonalnosti matrice.

rotacioni kvaternion- radikalan, ali vrlo neintuitivan lijek protiv suvišnosti i degeneracije. Ovo je četverokomponentni objekat - ne broj, ne vektor, ne matrica. Kvaternion se može posmatrati iz dva ugla. Prvo, kao formalni zbir skalara i vektora , gdje su jedinični vektori osa (što, naravno, zvuči apsurdno). Drugo, kao generalizacija kompleksni brojevi, koji sada koristi ne jedan, već tri drugačije imaginarne jedinice (što zvuči ništa manje apsurdno). Kako je kvaternion povezan s rotacijom? Kroz Ojlerovu teoremu: tijelo se uvijek može prenijeti iz jedne date orijentacije u drugu jednom konačnom rotacijom kroz neki ugao oko neke ose s vektorom smjera. Ovi uglovi i osa mogu se kombinovati u kvaternion: . Kao matrica, kvaternion se može koristiti za direktnu transformaciju bilo kojeg vektora iz jednog koordinatnog sistema u drugi: . Kao što možete vidjeti, kvaterniona reprezentacija orijentacije također pati od redundancije, ali mnogo manje od matrične: postoji samo jedan dodatni parametar. Detaljan pregled kvaterniona je već bio na Habréu. Radilo se o geometriji i 3D grafici. Zanima nas i kinematika, jer brzina promjene kvaterniona mora biti povezana sa izmjerenom ugaonom brzinom. Odgovarajuća kinematička jednačina ima oblik , gdje se vektor također smatra kvaternionom sa nultim skalarnim dijelom.

Šeme filtera

Najnaivniji pristup izračunavanju orijentacije je da se naoružamo kinematičkom jednadžbom i prema njoj ažuriramo bilo koji skup parametara koji nam se sviđa. Na primjer, ako smo odabrali matricu rotacije, možemo napisati petlju sa nečim poput C += C * Omega * dt. Rezultat će biti razočaravajući. Žiroskopi, posebno MEMS, imaju velike i nestabilne nulte pomake - kao rezultat toga, čak i u potpunom mirovanju, izračunata orijentacija će imati beskonačno akumulirajuću grešku (drift). Svi trikovi koje su izmislili Mahoney, Madgwick i mnogi drugi, uključujući i mene, bili su usmjereni na kompenzaciju ovog pomaka uključivanjem mjerenja s akcelerometara, magnetometara, GNSS prijemnika, zaostajanja, itd. Tako je rođena čitava porodica orijentacionih filtera zasnovanih na jednostavnom osnovnom principu.

Osnovni princip. Da bi se kompenzirao orijentacijski pomak, potrebno je ugaonoj brzini mjerenoj žiroskopima dodati dodatnu kontrolnu ugaonu brzinu konstruisanu na osnovu vektorskih mjerenja drugih senzora. Kontrolni vektor ugaone brzine treba da teži da uskladi pravce merenih vektora sa njihovim poznatim pravim pravcima.

Ovdje leži potpuno drugačiji pristup nego u konstrukciji korektivnog termina Kalmanovog filtera. Glavna razlika je u tome što je kontrolna ugaona brzina - nije termin, već faktor sa procijenjenom vrijednošću (matrica ili kvaternion). Ovo rezultira važnim prednostima:

• Filter za procjenu može se napraviti za samu orijentaciju, a ne za mala odstupanja orijentacije od one koju daju žiroskopi. U ovom slučaju, procijenjene vrijednosti će automatski zadovoljiti sve zahtjeve koje nameće problem: matrica će biti ortogonalna, kvaternion će biti normaliziran.
• Fizičko značenje kontrolne ugaone brzine je mnogo jasnije od korektivnog termina u Kalmanovom filteru. Sve manipulacije se rade sa vektorima i matricama u uobičajenom trodimenzionalnom fizičkom prostoru, a ne u apstraktnom višedimenzionalnom prostoru stanja. Ovo uvelike pojednostavljuje preciziranje i podešavanje filtera, a kao bonus, omogućava vam da se riješite velikih matrica i teških matričnih biblioteka.

Sada da vidimo kako se ova ideja implementira u određenim opcijama filtera.

Mahoney filter. Sva zadivljujuća matematika Mahoneyjevog originalnog članka napisana je da opravda jednostavne jednačine (32). Hajde da ih prepišemo u našoj notaciji. Ako zanemarimo procjenu nultih pomaka žiroskopa, tada će ostati dvije ključne jednadžbe - stvarna kinematička jednadžba za matricu rotacije (sa kontrolnom kutnom brzinom u obliku matrice) i zakon formiranja same te brzine u oblik vektora. Pretpostavimo radi jednostavnosti da nema ubrzanja ili magnetnih hvatača, a zahvaljujući tome, mjerenja ubrzanja su nam dostupna slobodan pad od akcelerometara i jačine Zemljinog magnetnog polja iz magnetometara. Oba vektora mjere se senzorima u povezanom koordinatnom sistemu, a u geografskom sistemu se pouzdano zna njihov položaj: usmjeren je prema gore, na magnetni sjever. Tada će jednadžbe Mahoneyjevog filtera izgledati ovako:

Pogledajmo pomno drugu jednačinu. Prvi član na desnoj strani je vektorski proizvod. Prvi faktor u njemu je izmjereno gravitacijsko ubrzanje, drugi je istinski. Pošto faktori moraju biti u istom koordinatnom sistemu, drugi faktor se pretvara u pridruženi sistem množenjem sa . Ugaona brzina, konstruisana kao vektorski proizvod, okomita je na ravan vektora množenja. Omogućava vam da rotirate izračunatu poziciju pridruženog koordinatnog sistema dok se vektori množitelja ne poklope u smjeru - tada će se vektorski proizvod nulirati i rotacija će se zaustaviti. Koeficijent postavlja rigidnost takve povratne informacije. Drugi član izvodi sličnu operaciju sa magnetnim vektorom. U stvari, Mahoney filter utjelovljuje dobro poznatu tezu: poznavanje dva nekolinearna vektora u dva različita koordinatna sistema omogućava jedinstveno vraćanje međusobne orijentacije ovih sistema. Ako postoji više od dva vektora, to će dati korisnu redundantnost mjerenja. Ako postoji samo jedan vektor, onda jedan rotacijski stepen slobode (kretanje oko ovog vektora) ne može biti fiksiran. Na primjer, ako je dat samo vektor, tada se može ispraviti pomak pri kotrljanju i nagibu, ali ne i skretanje.

Naravno, u Mahoney filteru nije potrebno koristiti matricu rotacije. Postoje i nekanonske varijante kvaterniona.

Virtuelna žiro platforma. U Mahoney filteru, primijenili smo ugaonu brzinu upravljanja na spregnuti koordinatni sistem. Ali možete ga primijeniti na izračunatu poziciju geografskog koordinatnog sistema. Kinematska jednadžba tada poprima oblik

Pokazalo se da takav pristup otvara put vrlo plodnim fizičkim analogijama. Dovoljno je prisjetiti se čime je počela žiroskopska tehnologija - smjerovima i inercijskim navigacijskim sistemima baziranim na žiro-stabiliziranoj platformi u kardanskom ovjesu.

www.theairlinepilots.com

Zadatak platforme je bio da materijalizuje geografski koordinatni sistem. Orijentacija nosača je mjerena u odnosu na ovu platformu pomoću senzora kuta na okvirima ovjesa. Ako su žiroskopi driftali, onda je platforma odlutala za njima, a greške su se nakupljale u očitavanju senzora ugla. Kako bismo otklonili ove greške, uveli smo Povratne informacije od akcelerometara instaliranih na platformi. Na primjer, odstupanje platforme od horizonta oko sjeverne ose je opaženo akcelerometrom istočne ose. Ovaj signal je omogućio postavljanje kontrolne ugaone brzine, koja vraća platformu na horizont.

Iste vizualne koncepte možemo koristiti u našem problemu. Napisanu kinematičku jednačinu tada treba čitati na sljedeći način: brzina promjene orijentacije je razlika između dva rotacijskim pokretima- apsolutno kretanje nosača (prvi član) i apsolutno kretanje virtuelne žiroplatforme (drugi član). Analogija se može proširiti na zakon formiranja kontrolne ugaone brzine. Vektor utjelovljuje očitavanja akcelerometara koji navodno stoje na žiro platformi. Zatim, iz fizičkih razmatranja, možemo napisati:

Potpuno isti rezultat mogao se postići formalno množenjem vektora u duhu Mahoney filtera, ali sada ne u povezanom, već u geografskom koordinatnom sistemu. Da li je to samo neophodno?

Prvi nagoveštaj korisne analogije između platforme i inercijalne navigacije se pojavljuje u drevnom Boeingovom patentu. Zatim je ovu ideju aktivno razvio Salychev, a nedavno i ja. Očigledne prednosti ovog pristupa:

• Kontrolna ugaona brzina se može formirati na osnovu razumljivih fizičkih principa.
• Naravno, razdvojeni su horizontalni i kursni kanal, koji se veoma razlikuju po svojim svojstvima i metodama korekcije. U Mahoney filteru se miješaju.
• Pogodno je kompenzirati utjecaj ubrzanja korištenjem GNSS podataka, koji su dati u geografskim, a ne povezanim osama.
• Algoritam je lako generalizirati na slučaj visokoprecizne inercijalne navigacije, gdje se mora uzeti u obzir oblik i rotacija Zemlje. Nemam pojma kako to učiniti u Mahoney shemi.

Madgwick filter. Madgwick je odabrala teži put. Ako je Mahoney, očigledno, intuitivno došao do svoje odluke, a zatim je matematički opravdao, onda se Madgwick od samog početka pokazao kao formalista. On se obavezao da će riješiti problem optimizacije. On je ovako obrazložio. Postavite orijentaciju na kvaternion rotacije. U idealnom slučaju, izračunati smjer nekog mjerenog vektora (da ga imamo) poklapa se sa pravim. Onda će biti. U stvarnosti, to nije uvijek izvodljivo (naročito ako postoji više od dva vektora), ali možete pokušati minimizirati odstupanje od tačne jednakosti. Da bismo to učinili, uvodimo kriterij minimizacije

Minimizacija zahtijeva gradijentno spuštanje - kretanje u malim koracima u smjeru suprotnom od gradijenta, tj. suprotno od najbržeg povećanja funkcije . Inače, Madgwick griješi: u sva svoja djela uopće ne ulazi i uporno piše umjesto , iako u stvari tačno izračunava .

Gradijentni silazak na kraju dovodi do sljedećeg uvjeta: da biste kompenzirali orijentacijski pomak, trebate dodati stopi promjene kvaterniona iz kinematičke jednadžbe novi negativni član proporcionalan:

Ovdje Madgwick malo odstupa od našeg "osnovnog principa": on dodaje termin korekcije ne ugaonoj brzini, već brzini promjene kvaterniona, a to nije potpuno ista stvar. Kao rezultat toga, može se ispostaviti da ažurirani kvaternion više neće biti jedinica i, shodno tome, izgubit će sposobnost predstavljanja orijentacije. Stoga je za Madgwick filter vještačka normalizacija kvaterniona vitalna operacija, dok je za druge filtere poželjna, a ne opciona.

Utjecaj ubrzanja

Do sada se pretpostavljalo da nema pravih ubrzanja i da akcelerometri mjere samo ubrzanje slobodnog pada. To je omogućilo da se dobije vertikalni standard i da se uz njegovu pomoć kompenzuje pomak kotrljanja i nagiba. Međutim, u opštem slučaju, akcelerometri, bez obzira na njihov princip rada, mjere prividno ubrzanje- vektorska razlika pravog ubrzanja i ubrzanja slobodnog pada. Smjer prividnog ubrzanja se ne poklapa s vertikalom, a greške zbog ubrzanja pojavljuju se u procjenama kotrljanja i nagiba.

Ovo se lako može ilustrovati upotrebom analogije virtuelne žiro platforme. Njegov sistem korekcije je projektovan tako da se platforma zaustavlja u ugaonom položaju u kojem se poništavaju signali akcelerometara koji su navodno na njoj instalirani, tj. kada izmjereni vektor postane okomit na ose osjetljivosti akcelerometara. Ako nema ubrzanja, ova pozicija se poklapa sa horizontom. Kada dođe do horizontalnih ubrzanja, žiroplatforma odstupa. Možemo reći da je žiroskopska platforma slična jako prigušenom klatnu ili visku.

U komentarima na post o Majwick filteru bljesnulo je pitanje da li je moguće nadati se da je ovaj filter manje podložan ubrzanjima od, na primjer, Mahoney filtera. Nažalost, svi ovdje opisani filteri rade na istim fizičkim principima i stoga pate od istih problema. Ne možete prevariti fiziku matematikom. Šta onda učiniti?

Najjednostavniji i najgrublji metod izmišljen je još sredinom prošlog stoljeća za vertikalni žiroskop aviona: smanjiti ili potpuno resetirati kontrolnu kutnu brzinu u prisustvu ubrzanja ili ugaone brzine kursa (što ukazuje na ulazak u zaokret). Ista metoda se može prenijeti na trenutne sisteme za spajanje. U ovom slučaju, ubrzanja treba suditi prema vrijednostima, a ne , koje su u zavoju same po sebi nula. Međutim, po veličini nije uvijek moguće razlikovati prava ubrzanja od projekcija ubrzanja slobodnog pada zbog samog nagiba žiro platforme koji treba eliminirati. Stoga metoda djeluje nepouzdano - ali ne zahtijeva nikakve dodatne senzore.

Precizniji metod se zasniva na upotrebi eksternih merenja brzine sa GNSS prijemnika. Ako je brzina poznata, onda se može numerički razlikovati i dobiti pravo ubrzanje. Tada će razlika biti potpuno jednaka bez obzira na kretanje nosača. Može se koristiti kao vertikalni standard. Na primjer, može se postaviti kontrolne ugaone brzine žiroplatforme u obliku

Nulti pomaci senzora

Tužna karakteristika potrošačkih žiroskopa i akcelerometara je velika nestabilnost nultih pomaka u vremenu i temperaturi. Da biste ih eliminisali, samo jedna fabrička ili laboratorijska kalibracija nije dovoljna - potrebno je ponovo procijeniti tijekom rada.

Žiroskopi. Hajde da se pozabavimo nultim pomacima žiroskopa. Izračunati položaj pridruženog koordinatnog sistema se udaljava od svog pravog položaja sa ugaonom brzinom koju određuju dva suprotstavljena faktora - nulti pomaci žiroskopa i kontrolna ugaona brzina: . Ako je sistem korekcije (na primjer, u Mahoney filteru) uspio zaustaviti zanošenje, tada će biti u stabilnom stanju. Drugim riječima, kontrolna kutna brzina sadrži informaciju o nepoznatoj aktivnoj perturbaciji. Stoga se možete prijaviti kompenzatorna procjena: ne znamo direktno veličinu smetnje, ali znamo koja je korektivna radnja potrebna da se uravnoteži. Ovo je osnova za procjenu pomaka nule žiroskopa. Na primjer, Mahoneyjev rezultat se ažurira u skladu sa zakonom

Međutim, njegov rezultat je čudan: procjene dosežu 0,04 rad/s. Takva nestabilnost nultih pomaka se ne dešava ni kod najgorih žiroskopa. Pretpostavljam da je problem u tome što Mahoney ne koristi GNSS ili druge eksterne senzore - i pati od efekata ubrzanja u punoj mjeri. Samo na okomitoj osi, gdje ubrzanja ne štete, procjena izgleda manje-više razumno:

Mahony et al., 2008

akcelerometri. Procijeniti nulte pomake akcelerometara je mnogo teže. Informacije o njima moraju se izvući iz iste kontrolne ugaone brzine. Međutim, u pravolinijsko kretanje učinak nultih pomaka akcelerometara ne razlikuje se od nagiba nosača ili neusklađenosti instalacije senzorske jedinice na njemu. Ne stvaraju se aditivi za akcelerometre. Aditiv se pojavljuje samo za vrijeme skretanja, što omogućava odvajanje i neovisnu procjenu grešaka žiroskopa i akcelerometara. Primjer kako se to može učiniti je u mom članku. Evo slika odatle:

Umjesto zaključka: šta je sa Kalmanovim filterom?

Ne sumnjam da će ovdje opisani filteri gotovo uvijek imati prednost u odnosu na tradicionalni Kalman filter u smislu brzine, kompaktnosti koda i lakoće prilagođavanja - za to su i stvoreni. Što se tiče tačnosti procjene, ovdje nije sve tako jednoznačno. Vidio sam neuspješno dizajnirane Kalmanove filtere, koji su po preciznosti primjetno gubili u odnosu na filter sa virtuelnom žiro platformom. Madgwick je također argumentirao prednosti svog filtera u odnosu na neki Kalman procjenjuje. Međutim, za isti problem procjene orijentacije, može se izgraditi najmanje desetak različitih krugova Kalmanovog filtera, a svaki će imati beskonačan broj opcija podešavanja. Nemam razloga da mislim da će Mahoney ili Madgwick filter biti precizniji najbolji mogući Kalman filteri. I naravno, Kalmanov pristup će uvijek imati prednost univerzalnosti: ne nameće nikakva striktna ograničenja na specifična dinamička svojstva sistema koji se evaluira.