INS w UAV

Inertial Navigation System wyznacza orientację, prędkość i pozycję na podstawie pomiarów żyroskopów oraz akcelerometrów, bez konieczności ciągłego odbierania zewnętrznego sygnału. W typowym UAV stosuje się INS strapdown: IMU jest sztywno związane z kadłubem, a obrót pomiaru przyspieszenia do układu nawigacyjnego wykonuje algorytm. Mechanicznie jest to prostsze od platformy kardanowej, lecz wszystkie obroty, drgania, błędy montażu i ograniczenia dynamiki trafiają bezpośrednio do sensorów.

INS nie jest synonimem IMU. IMU dostarcza przyrost kąta i prędkości albo surową prędkość kątową oraz siłę właściwą. INS dodaje inicjalizację, kalibrację, mechanizację równań ruchu, model grawitacji, układy współrzędnych, estymację biasów, aiding oraz ocenę jakości. Samo dwukrotne całkowanie akcelerometru MEMS bez tych elementów daje szybko rozbieżną pozycję.

Spis treści#

• IMU, AHRS i INS
• Strapdown a układ kardanowy
• Co mierzą sensory
• Stan nawigacyjny
• Układy współrzędnych
• Mechanizacja orientacji
• Mechanizacja prędkości
• Mechanizacja pozycji
• Grawitacja, obrót Ziemi i transport rate
• Delta angle i delta velocity
• Coning i sculling
• Inicjalizacja
• Alignment roll i pitch
• Heading
• Bias akcelerometru
• Bias żyroskopu
• Skala, nieortogonalność i temperatura
• Jak szybko rośnie błąd
• Dlaczego niskokosztowy MEMS nie jest samodzielnym INS długiego trwania
• Aiding i obserwowalność
• Integracja luźna, ścisła i głęboka
• GNSS
• Barometr, magnetometr i prędkość powietrza
• Optical flow, visual odometry i lidar
• EKF error-state
• Innowacje i integralność
• Utrata aiding
• Redundancja IMU
• Architektura czasu i opóźnienia
• Implementacja
• Testy
• Typowe błędy
• Kryteria odbioru
• Powiązane tematy
• Przypisy

IMU, AHRS i INS#

AHRS może utrzymywać dobrą orientację, lecz nie znać pozycji. INS może przez pewien czas propagować pozycję bez GNSS, ale jej niepewność rośnie. Autopilot często używa jednego estymatora, który spełnia role AHRS i aided INS, publikując lokalny oraz globalny stan.

Określenie „9-osiowa IMU” bywa marketingowym skrótem dla żyroskopu, akcelerometru i magnetometru. Magnetometr nie mierzy osi ruchu i nie czyni sensora INS. Jego wskazanie zależy od pola Ziemi, zaburzeń instalacji i kalibracji.

Strapdown a układ kardanowy#

W platformie kardanowej zespół sensorów jest mechanicznie utrzymywany w zadanej orientacji. Żyroskopy sterują gimbalami, a akcelerometry nie doświadczają pełnego zakresu obrotów kadłuba. System jest złożony mechanicznie, ciężki i kosztowny, ale może mieć zalety dokładnościowe.

W strapdown:

IMU związane z body frame
      ↓ gyro delta-angle
aktualizacja orientacji C_b^n / quaternion
      ↓ accel delta-velocity w body
obrót do navigation frame
      ↓ + gravity/Coriolis
prędkość → pozycja

Sensor widzi pełną dynamikę platformy. Gyro musi objąć maksymalne rate bez saturacji, akcelerometr — manewry, drgania i uderzenia. Algorytm musi poprawnie składać małe obroty i przyrosty prędkości.

UAV niemal zawsze używa strapdown ze względu na masę, rozmiar i cenę MEMS. Jakość zależy wtedy od synchronizacji próbek, kalibracji i obliczeń równie mocno jak od samego sensora.

Co mierzą sensory#

Żyroskop mierzy prędkość kątową obudowy względem przestrzeni inercjalnej, wyrażoną w osiach sensora, z biasem, szumem, błędem skali i nieortogonalnością.

Akcelerometr nie mierzy „przyspieszenia kinematycznego” wprost. Mierzy siłę właściwą:

f = a - g

zależnie od przyjętych znaków i układów. W spoczynku na stole akcelerometr pokazuje około 1 g, ponieważ podłoże wywiera siłę podtrzymującą przeciwną do grawitacji. W swobodnym spadku idealny akcelerometr wskazuje blisko zera, choć obiekt przyspiesza względem Ziemi.

INS obraca siłę właściwą do układu nawigacyjnego, dodaje model grawitacji oraz poprawki obracającej się Ziemi, a wynik całkuje. Błąd orientacji powoduje, że ogromny wektor grawitacji przecieka do poziomego przyspieszenia.

Stan nawigacyjny#

Minimalny stan nominalny:

x = [q_nb, v_n, p_n, b_g, b_a]

gdzie:

• q_nb opisuje orientację body względem navigation frame;
• v_n jest prędkością w układzie nawigacyjnym;
• p_n pozycją lokalną lub geodezyjną;
• b_g biasem żyroskopu;
• b_a biasem akcelerometru.

Rozszerzenia obejmują skalę sensorów, pole magnetyczne i jego zakłócenie w body, wiatr, bias barometru, offsety anten/sensorów, terrain height oraz opóźnienia.

Stan nominalny i covariance nie są tym samym. Covariance opisuje szacowaną niepewność oraz korelacje. Filtr może publikować liczbę pozycji mimo braku aiding, lecz covariance powinna rosnąć, a system sterowania musi mieć progi jakości.

Układy współrzędnych#

Najczęściej występują:

• body: osie związane z kadłubem;
• sensor: osie fizycznego układu, nie zawsze zgodne z body;
• NED: north-east-down lokalnie;
• ENU: east-north-up w ekosystemie robotycznym;
• ECEF: kartezjański układ związany z Ziemią;
• geodezyjny: szerokość, długość i wysokość względem elipsoidy.

Transformacja sensor→body jest stałą kalibracją montażu. Body→navigation zmienia się z attitude. Pomylenie aktywnego obrotu wektora z pasywną zmianą bazy albo q_nb z q_bn daje poprawnie wyglądające wyniki tylko w części orientacji.

Konwencja kwaternionu, kolejność mnożenia, handedness i znaki osi muszą być zapisane w jednym dokumencie interfejsu. Testy używają orientacji 90° i 180° wokół każdej osi, nie wyłącznie poziomu.

Mechanizacja orientacji#

IMU dostarcza przyrost kąta Δθ_ib^b. Po odjęciu biasu i uwzględnieniu obrotu układu nawigacyjnego tworzy się mały kwaternion:

δq ≈ [1, 0.5 Δθ_x, 0.5 Δθ_y, 0.5 Δθ_z]
q_k+1 = normalize(q_k ⊗ δq)

Dla większych przyrostów używa się dokładnej funkcji wykładniczej/quaternionu z sin(|Δθ|/2). Kolejność mnożenia zależy od definicji q i musi być zgodna z transformacją.

Na obracającej się Ziemi orientacja body względem NED zmienia się także wskutek earth rate i transport rate. W małym UAV oraz krótkim czasie można część składników zaniedbać świadomie, ale nie należy nazywać uproszczonego integratora pełnym INS globalnym.

Kwaternion jest normalizowany z kontrolą normy. Jeśli norma jest NaN, bliska zeru lub skokowo błędna, nie wykonuje się normalizacji w ciemno; estimator przechodzi w stan awarii.

Mechanizacja prędkości#

W uproszczonym lokalnym układzie:

a_n = C_b^n (f_b - b_a) + g_n - coriolis_terms
v_n,k+1 = v_n,k + a_n Δt

C_b^n pochodzi z attitude w chwili zgodnej z próbką akcelerometru. Użycie końcowej orientacji do całego interwału bez korekty obrotu tworzy błąd sculling.

Pełniejsza mechanizacja NED uwzględnia:

v_dot^n = C_b^n f^b + g^n - (2ω_ie^n + ω_en^n) × v^n

gdzie ω_ie to obrót Ziemi, a ω_en transport rate układu lokalnego. Znaki zależą od konwencji.

Przy UAV lecącym krótko i wolno Coriolis może być mały wobec biasu MEMS, ale przy dłuższym czasie i większej prędkości staje się systematyczny. Model dobiera się do wymagań, nie do wygody implementacji.

Mechanizacja pozycji#

W lokalnym NED można aktualizować pozycję metryczną:

p_k+1 = p_k + 0.5 (v_k + v_k+1) Δt

Dla globalnego INS prędkości NED przelicza się na zmiany szerokości, długości i wysokości z promieniami krzywizny elipsoidy. W pobliżu biegunów lokalna mechanizacja wymaga szczególnej obsługi; ECEF unika singularności długości geograficznej.

Pozycja IMU nie zawsze jest pozycją środka kadłuba, anteny GNSS lub kamery. Offset lever arm oraz prędkość wynikająca z obrotu muszą być uwzględnione, zwłaszcza przy dużym ramieniu i szybkim yaw.

Integracja trapezowa ogranicza część błędu dyskretyzacji wobec prostego Eulera. Nie usuwa biasu. Podwojenie częstotliwości próbkowania nie poprawi systematycznego offsetu sensora bez kalibracji/aiding.

Grawitacja, obrót Ziemi i transport rate#

Grawitacja zależy od szerokości geograficznej i wysokości. Dla małego obszaru wystarczy lokalny model; dla globalnej mechanizacji używa się modelu zgodnego z elipsoidą. Stałe 9.81 jest przybliżeniem, nie uniwersalną wartością.

Earth rate ma około 15°/h. Dla wysokiej klasy gyro jest sygnałem używanym do gyrocompass alignment. Dla taniego MEMS bias może być większy od earth rate, więc kierunek północy z samego gyro jest praktycznie nieobserwowalny na wymaganym czasie.

Transport rate wynika z ruchu lokalnego układu NED po krzywej Ziemi. Coriolis zależy od prędkości. Jeśli system działa kilkanaście sekund w hali, uproszczenie może być uzasadnione. Jeśli ma lecieć godzinami i globalnie, pełne składniki są wymagane.

Dokument wymagań powinien jawnie wypisać zaniedbane efekty i przewidywany błąd, zamiast używać etykiety „INS” dla każdego integratora.

Delta angle i delta velocity#

Współczesny estimator często nie przyjmuje chwilowego gyro rad/s i accel m/s², lecz przyrosty z dokładnym interwałem:

Δθ = ∫ ω dt
Δv = ∫ f dt

IMU lub sterownik może akumulować próbki w wyższej częstotliwości, wykonywać korekcję coning/sculling i przekazywać delty np. 100–1000 Hz. PX4 EKF2 wymaga danych delta-angle/delta-velocity oraz opisuje potrzebę coning correction.

Każdy pakiet ma:

• timestamp środka/końca interwału;
• delta_angle_dt i delta_velocity_dt;
• status saturacji/FIFO;
• identyfikator IMU;
• korekcję temperatury/kalibracji;
• licznik sekwencji.

Zakładanie stałego dt z konfiguracji przy jitterze prowadzi do błędu skali integracji. Rzeczywisty czas pochodzi z hardware timebase i synchronizacji sensora.

Coning i sculling#

Coning powstaje, gdy małe obroty wokół różnych osi w obrębie interwału nie komutują. Suma wektorów Δθ1 + Δθ2 pomija składnik krzyżowy. Dla dwóch próbek pierwsza korekcja ma postać zależną od konwencji, często zawierającą:

Δθ_corrected ≈ Δθ1 + Δθ2 + (2/3)(Δθ1 × Δθ2)

Dokładny współczynnik zależy od algorytmu i sposobu próbkowania. Nie należy kopiować wzoru bez zgodności z mechanizacją.

Sculling dotyczy przyrostu prędkości przy jednoczesnym obrocie i zmianie siły właściwej. Korekcje wykorzystują iloczyny krzyżowe delta-angle i delta-velocity. Są ważne przy wibracjach oraz szybkich manewrach.

Vibration coning może generować pozorny dryf attitude nawet wtedy, gdy średnia prędkość kątowa jest bliska zeru. Dlatego anti-aliasing, synchroniczne próbki i poprawne delta są częścią nawigacji, nie kosmetyką filtra.

Inicjalizacja#

INS wymaga początkowych:

• czasu i pozycji/origin;
• prędkości;
• roll, pitch i heading;
• biasów oraz covariance;
• orientacji IMU względem body;
• modelu grawitacji i układu nawigacyjnego.

Na nieruchomym pojeździe średni akcelerometr daje kierunek pionu, lecz nie odróżnia grawitacji od rzeczywistego przyspieszenia. Dlatego alignment wymaga spoczynku albo dodatkowych obserwacji. GNSS daje pozycję i przy dłuższym ruchu kurs prędkości; nie daje headingu w spoczynku z pojedynczej anteny.

Turn-on bias może zmieniać się po każdym włączeniu. Estimator potrzebuje czasu na zbieżność przed uzbrojeniem. Pre-arm checks oceniają bias, innowacje, drgania i zgodność sensorów.

Zapisanie biasu z poprzedniego lotu może przyspieszyć start, lecz temperatura i naprężenia zmieniają go. Taka wartość jest prior z niepewnością, nie pewnikiem.

Alignment roll i pitch#

Przy braku przyspieszenia liniowego znormalizowany akcelerometr wskazuje pion. Roll i pitch można wyznaczyć z geometrii wektora grawitacji. Yaw pozostaje nieobserwowalny, ponieważ obrót wokół pionu nie zmienia tego wektora.

Warunki spoczynku:

• norma gyro poniżej progu;
• wariancja accel/gyro w oknie poniżej limitu;
• norma accel bliska lokalnemu g;
• brak saturacji i clip;
• temperatura w dozwolonym zakresie.

Na pokładzie kołyszącego się statku, pojazdu albo w ręce operatora warunek może nie być spełniony. Filtr nie powinien „nauczyć” przyspieszenia jako biasu.

Błąd tilt jest szczególnie groźny: mały kąt powoduje rzut grawitacji na poziom. 1° to około 0,171 m/s² fałszywego poziomego przyspieszenia (g sin 1°).

Heading#

Źródła yaw/heading:

• magnetometr po kalibracji i kompensacji pola pokładowego;
• dual-antenna GNSS moving baseline;
• course over ground przy wystarczającej prędkości i małym sideslip;
• visual odometry/mapa;
• external vision/mocap;
• gyrocompassing dla sensora o odpowiedniej klasie;
• aerodynamiczne constraints oraz prędkość powietrza w płatowcu.

Magnetometr przy przewodach baterii i silnikach może być gorszy niż propagacja gyro. Estimator monitoruje innowacje pola oraz zgodność jego normy/kierunku z modelem. Nie należy bezwarunkowo korygować pełnego attitude skażonym magiem.

Course GNSS nie jest headingiem kadłuba podczas zawisu, lotu bokiem albo w silnym wietrze. Dual GNSS mierzy baseline niezależnie od ruchu, ale wymaga poprawnej geometrii anten i rozwiązania ambiguities.

Yaw bias jest słabo obserwowalny bez ruchu lub bez niezależnego headingu. Wymuszone manewry kalibracyjne poprawiają obserwowalność, lecz muszą być bezpieczne.

Bias akcelerometru#

Stały bias b_a po odjęciu grawitacji daje w uproszczeniu:

δv(t) = b_a t
δp(t) = 0.5 b_a t²

Dla zaledwie 0,01 m/s²:

To model jednej osi, stałego biasu i idealnej orientacji. Realny błąd zawiera szum, temperaturę i błędy tilt, więc może być większy lub częściowo się znosić.

Bias jest estymowany jako stan random walk/first-order process z aiding. Gdy zewnętrzne obserwacje znikają, jego niepewność rośnie. Nie należy zbyt agresywnie „uczyć biasu” podczas dużego manewru, bo model pomyli nonlinearity i przyspieszenie z offsetem.

Bias żyroskopu#

Stały bias gyro b_g tworzy błąd kąta około:

δθ(t) ≈ b_g t

Jeśli błąd tilt rzutuje grawitację poziomo, krótkookresowo:

δa_horizontal ≈ g b_g t
δv_horizontal ≈ 0.5 g b_g t²
δp_horizontal ≈ (1/6) g b_g t³

Dla biasu 0,1°/s (0,001745 rad/s) po 30 s przybliżenie daje błąd tilt 3° i poziomy błąd pozycji rzędu dziesiątek metrów. Mały gyro bias jest więc często bardziej destrukcyjny od podobnie wyglądającego szumu.

W dłuższym czasie pełna dynamika INS, curvature i aiding zmieniają prosty wzrost sześcienny. Wzór dobrze pokazuje jednak, dlaczego „tani IMU + podwójna integracja” szybko odpływa.

Bias yaw nie rzutuje grawitacji bezpośrednio, ale obraca poziome przyspieszenia i kierunek prędkości. Podczas manewru prowadzi do dużych błędów pozycji.

Skala, nieortogonalność i temperatura#

Model sensora:

y = M S x + b(T, t) + noise + nonlinearity

S opisuje skalę osi, M nieortogonalność i cross-axis, a b zależy od temperatury oraz czasu. Kalibracja sześciopozycyjna akcelerometru wyznacza bias/skale, ale dokładna macierz potrzebuje większej liczby orientacji i dobrego wzorca.

Gyro kalibruje się w spoczynku oraz na stole obrotowym dla skali. Bez stołu można oszacować bias i temperaturę, ale skala dynamiczna pozostaje słabiej znana.

Kompensacja temperatury używa wielu punktów podczas kontrolowanego nagrzewania/chłodzenia. Sensor nie powinien kondensować wilgoci. Model wielomianowy lub tabela ma zakres obowiązywania; poza nim health spada.

Naprężenie PCB, klej, śruby i upadek mogą zmienić kalibrację. Po naprawie/montażu oraz uderzeniu wykonuje się ponowną weryfikację.

Jak szybko rośnie błąd#

Nie istnieje jedna wartość „dryft INS na minutę” bez specyfikacji sensora, ruchu, temperatury, alignmentu, mechanizacji i aiding. Błędy mają różne prawa czasowe:

Covariance filtera przewiduje część wzrostu, ale tylko dla poprawnego modelu szumu. Nieujęte clipping, aliasing, wibracje i temperatura powodują overconfidence.

Monte Carlo z modelem sensorów i replay rzeczywistych logów są lepsze niż jeden lot. Wynik raportuje percentyle, nie tylko średnią.

Dlaczego niskokosztowy MEMS nie jest samodzielnym INS długiego trwania#

Tani MEMS ma bias większy i mniej stabilny niż nawigacyjne FOG/RLG. Earth rate jest często mniejszy od biasu, więc autonomiczny north alignment jest nierealny. Temperatura, drgania i g-sensitivity zmieniają offset w locie.

W praktyce MEMS UAV jest rdzeniem propagacji między zewnętrznymi obserwacjami. GNSS, barometr, magnetometr, optical flow, airspeed, vision lub lidar ograniczają stany. Gdy wszystkie znikają, system kontynuuje tylko przez czas odpowiadający wymaganiom i rzeczywistemu budżetowi błędu.

Stwierdzenie „INS działa bez GNSS” jest prawdziwe funkcjonalnie, lecz nie mówi jak długo pozycja pozostaje użyteczna. Wymaganie powinno brzmieć np. „95% błędu poziomego < X m po Y s bez aiding w określonym profilu ruchu i temperaturze”.

Aiding i obserwowalność#

Aiding dostarcza obserwacji stanu lub jego kombinacji:

• GNSS: pozycja/prędkość/czas;
• barometr/rangefinder: wysokość lub odległość;
• magnetometr: kierunek pola;
• optical flow: prędkość kątowa obrazu związana z ruchem;
• visual odometry: względny ruch/poza;
• lidar/radar odometry: ruch względem środowiska;
• airspeed: prędkość względem powietrza;
• zero-velocity update: prędkość zero podczas pewnego postoju;
• constraints pojazdu: np. brak bocznej prędkości UGV.

Obserwowalność zależy od ruchu. Bias akcelerometru i tilt mogą być trudne do rozdzielenia w spoczynku. Heading staje się obserwowalny przy przyspieszeniu i niezależnej prędkości/pozycji. Bias magnetometru wymaga zmian orientacji.

Dodanie sensora do wektora pomiarowego nie gwarantuje obserwowalności. Analiza Jacobianu, symulacja i eksperymenty manewrowe pokazują, które stany rzeczywiście zbiegną.

Integracja luźna, ścisła i głęboka#

Loose coupling łączy gotowe rozwiązanie pozycji/prędkości GNSS z INS. Jest prostsze, ale przy małej liczbie satelitów odbiornik może przestać publikować fix i filtr traci surowe informacje.

Tight coupling fuzjonuje pseudoodległości, Doppler/carrier i stan INS we wspólnym filtrze. Może korzystać z pojedynczych obserwacji satelitów i lepiej monitorować niespójności, lecz wymaga dokładnych modeli zegara, anteny, opóźnień i jakości.

Deep coupling ingeruje w pętle śledzenia odbiornika GNSS z pomocą INS. Jest złożone i zależne od receivera. Dla typowego DIY UAV nie jest pierwszym etapem.

Podobne pojęcia dotyczą vision: loose coupling fuzjonuje gotową pozę VO, tight coupling może fuzjonować cechy/pomiary kamery razem ze stanem inercyjnym.

GNSS#

GNSS ogranicza pozycję i prędkość, a PPS/timing może synchronizować domeny czasu. Prędkość Doppler bywa dokładniejsza krótkookresowo niż różnicowanie pozycji.

Fuzja uwzględnia:

• lever arm antena–IMU;
• latency i timestamp pomiaru;
• covariance/accuracy odbiornika;
• fix type, satelity i DOP;
• consistency innovations;
• skoki rozwiązania i multipath;
• wysokość elipsoidalną kontra MSL;
• odrzucenie danych stale/duplicate.

GNSS może być błędny mimo formalnego fix. Innovation gating i porównanie z innymi sensorami wykrywa część problemów. Nagły reset INS do niepewnej pozycji może być gorszy niż kontrolowane dead reckoning.

PX4 opisuje eksperymentalny tryb dla GNSS degraded/denied, który wymaga alternatywnych źródeł pozycji/prędkości i dotyczy dropout, nie „czystego lotu bez GNSS od startu”. To ważne ograniczenie interpretacji.

Barometr, magnetometr i prędkość powietrza#

Barometr stabilizuje pion, ale cierpi na prop wash, temperaturę, pogodę i ciśnienie dynamiczne. Bias wysokości jest stanem. Rangefinder może wspierać nisko nad terenem, lecz mierzy odległość wzdłuż osi i wymaga kompensacji tilt.

Magnetometr ogranicza yaw/pole, ale instalacja jest krytyczna. Prąd i ferromagnetyki tworzą hard/soft iron oraz zależność od throttle. Test compass-motor mierzy wpływ napędu.

Airspeed w płatowcu wraz z modelem wiatru ogranicza prędkość względem powietrza. Nie daje bezpośrednio groundspeed ani pozycji. Przy pitot blockage obserwacja jest błędna; estimator musi mieć innovation checks.

Każdy sensor ma osobny timeout i status. Utrata barometru nie może być maskowana świeżym magnetometrem przez wspólną flagę „aiding OK”.

Optical flow, visual odometry i lidar#

Optical flow daje ruch obrazu. Do prędkości liniowej potrzebuje wysokości/range i kompensacji rotacji gyro. Tekstura, światło, motion blur i wysokość ograniczają działanie.

Visual odometry daje względny ruch kamery, czasem skalę z IMU/stereo/depth. Wymaga transformacji kamera–IMU i synchronizacji. Opóźnienie pipeline może być dziesiątki milisekund; filtr fuzjonuje pomiar na właściwym horizon i propaguje do teraz.

Lidar/radar odometry może działać w innych warunkach, ale także ma degeneracje geometryczne. Długi korytarz, jednolita płaszczyzna albo deszcz mogą osłabić osie.

INS zapewnia propagację wysokiej częstotliwości, a odometria koryguje dryf. Gdy odometria traci tracking, covariance rośnie. Nie wolno utrzymywać ostatniej pozy jako świeżego pomiaru.

EKF error-state#

Popularna architektura utrzymuje nominalny stan nieliniowy i mały error state:

δx = [δθ, δv, δp, δb_g, δb_a, ...]

IMU propaguje nominalny stan i covariance. Zewnętrzny pomiar tworzy innowację:

r = z - h(x)
S = HPHᵀ + R
K = PHᵀ S⁻¹
δx = K r

Error wstrzykuje się do nominalnego kwaternionu/prędkości/pozycji, a error state resetuje z odpowiednią transformacją covariance.

Covariance prediction używa modelu bias random walk i szumów IMU. Zbyt małe process noise daje overconfidence i odrzuca prawdziwe pomiary. Zbyt duże daje hałaśliwy stan. Parametry pochodzą z charakterystyki sensora/logów, nie losowego tuningu.

Joseph form covariance update poprawia stabilność numeryczną. Macierz jest symetryzowana i kontrolowana pod kątem dodatniej określoności/nieujemnych wariancji.

Innowacje i integralność#

Innowacja to różnica pomiar–predykcja. Normalized innovation squared lub test ratio porównuje ją z przewidywaną covariance. Pojedynczy outlier można odrzucić; seria wskazuje zły sensor, model, opóźnienie lub rozbieżność INS.

Monitoruje się osobno:

• GNSS position/velocity;
• height sources;
• magnetic field/yaw;
• optical flow/vision;
• airspeed;
• bias i covariance;
• vibration/clipping;
• time sync i stale data.

„EKF healthy” nie może być jednym bitem bez wyjaśnienia. Kontroler potrzebuje jakości attitude, local position, global position i velocity niezależnie.

Fault isolation porównuje redundantne sensory i innovation. Jeśli jeden GNSS odchodzi, majority/consistency może go wykluczyć. Dwa sensory nie tworzą większości; potrzebny jest model lub trzecie źródło.

Utrata aiding#

Po utracie GNSS/vision estimator nadal propaguje IMU. Kluczowe są:

• czas od ostatniego dobrego pomiaru;
• covariance i przewidywany growth;
• ostatnie biasy oraz temperatura;
• dostępne alternatywne źródła;
• profil ruchu i wymagany błąd;
• polityka controller/misji.

Nie wykonuje się natychmiastowego resetu do dowolnego nowego źródła. Najpierw sprawdza się spójność. Tryb dead reckoning ma ograniczony envelope i deadline. Po przekroczeniu jakości platforma może przejść do attitude-only, ograniczyć prędkość, lądować albo kontynuować bez pozycji zależnie od bezpiecznej koncepcji.

Odzyskanie aiding wymaga histerezy i stopniowej korekty. Skok pozycji może wygenerować duży setpoint. Estimator może korygować stan, a controller używa smooth reset counter/offset.

Redundancja IMU#

Wiele IMU może zapewnić:

• wykrywanie bias jump i saturacji;
• voting/selection;
• niezależne instancje estymatora;
• utrzymanie po awarii jednej magistrali;
• porównanie drgań lokalizacji.

Redundancja jest pozorna, jeśli sensory dzielą regulator, SPI, clock, footprint, temperaturę i błąd mechaniczny. Niezależność zasilania/magistral poprawia pokrycie.

Proste uśrednianie dwóch IMU może rozmyć awarię. Lepsze jest monitorowanie residual i wybór/parallel EKF. Przełączenie ma alignment offset oraz ciągłość attitude.

IMU diversity (różni producenci/zasady) ogranicza common-mode, ale zwiększa różnice filtrów i kalibracji. Kwalifikacja obejmuje każdy wariant BOM.

Architektura czasu i opóźnienia#

INS jest systemem czasu. Każda obserwacja musi być fuzjonowana w chwili wykonania. EKF często utrzymuje bufor stanów i działa na opóźnionym fusion horizon, a następnie propaguje wynik do bieżącego czasu.

Źródła opóźnienia:

• filtr cyfrowy IMU;
• FIFO i transfer magistrali;
• timestamp po stronie hosta;
• GNSS measurement latency;
• pipeline kamery;
• kolejki middleware;
• scheduler i logowanie.

Błąd czasu δt przy prędkości kątowej ω tworzy błąd orientacji ~ωδt, a przy prędkości v — błąd lever/position ~vδt. 20 ms przy 200°/s to 4° różnicy orientacji.

Synchronizacja hardware (PPS, trigger, capture) jest lepsza od czasu przybycia pakietu. Offset i drift zegarów są estymowane lub kalibrowane.

Implementacja#

Minimalny przepływ:

void ins_predict(const imu_delta_t *imu, ins_state_t *s)
{
    if (!imu_valid(imu) || !state_finite(s)) {
        ins_set_fault(s, INS_FAULT_INPUT);
        return;
    }

    vec3 dtheta = imu->delta_angle - s->gyro_bias * imu->dt_angle;
    vec3 dvel_b = imu->delta_velocity - s->accel_bias * imu->dt_velocity;

    dtheta = apply_coning_correction(dtheta, s->previous_delta_angle);
    quat q_mid = attitude_midpoint(s->q_nb, dtheta);
    s->q_nb = normalize_checked(quat_integrate(s->q_nb, dtheta));

    vec3 f_n = rotate_body_to_nav(q_mid, dvel_b) / imu->dt_velocity;
    vec3 a_n = f_n + gravity_model(s->position)
                 - coriolis(s->position, s->velocity);

    vec3 v_old = s->velocity;
    s->velocity += a_n * imu->dt_velocity;
    s->position = integrate_position(s->position, v_old, s->velocity,
                                     imu->dt_velocity);
    covariance_predict(s, imu);
}

To pseudokod. Pełna implementacja musi stosować spójną mechanizację deltas, sculling, earth/transport rate, lever arms, error-state covariance i dokładne jednostki. dt_angle i dt_velocity mogą się różnić.

Każda funkcja ma testy na orientacje graniczne, brak ruchu, stały obrót, stałe przyspieszenie i syntetyczne biasy. Replay używa tych samych binarnych danych co lot.

Testy#

Jednostkowe#

• rotacje 90/180° każdej osi;
• stały obrót i pełny obrót wracający do orientacji;
• brak ruchu przy różnych orientacjach;
• stałe przyspieszenie i analityczna trajektoria;
• accel bias z błędem t²;
• gyro bias z tilt i g-coupling;
• coning/sculling sekwencje;
• NaN, zero norm, saturacja i błędny dt;
• NED/ECEF/geodetic round-trip.

Monte Carlo#

Losuje się bias, random walk, scale, alignment, temperaturę, opóźnienia i initial state. Wynik porównuje empirical error z covariance. Filtr overconfident jest niebezpieczny nawet przy małym średnim błędzie.

Replay i HIL#

• rzeczywiste logi drgań;
• GNSS dropout i skok;
• mag interference zależne od throttle;
• baro prop wash;
• vision latency/dropout;
• alternatywne aiding w różnych kombinacjach;
• reset/przełączenie IMU;
• opóźnione i out-of-order wiadomości.

Lot próbny#

Najpierw pełne aiding i bezpieczny obszar. Utratę źródła symuluje się programowo z zachowaniem logu, nie przez ryzykowne manipulacje w locie. Kryteria abort opierają się na innovation, covariance, attitude disagreement i geofence testowym.

Typowe błędy#

Kryteria odbioru#

INS w UAV jest przede wszystkim kontrolowanym propagatorem stanu i niepewności między obserwacjami. Jego wartość nie polega na obietnicy „lotu bez GNSS”, lecz na dokładnym określeniu, jak długo i w jakim ruchu stan pozostaje wystarczająco dobry, jakie źródła go ograniczają oraz kiedy system ma obowiązek przejść do bezpieczniejszego trybu.

Powiązane tematy#

• IMU w dronie
• Układy współrzędnych w UAV
• Filtr Kalmana i EKF w UAV
• GNSS w UAV
• Magnetometr w UAV
• Barometr w UAV
• Optical flow w UAV
• Dead reckoning UAV

Przypisy#

1. US Coast Guard Navigation Center, GPS User Equipment — Inertial Navigation Systems, https://navcen.uscg.gov/sites/default/files/pubs/gps/gpsuser/gpsuser.pdf (dostęp: 15 sierpnia 2026).
2. US Coast Guard Navigation Center, Augmented GPS — Inertial Navigation Systems, https://www.navcen.uscg.gov/sites/default/files/pubs/gpsaug/auggps.pdf (dostęp: 15 sierpnia 2026).
3. NASA, Integrated INS/GPS Navigation from a Popular Perspective, https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20020041935.pdf (dostęp: 15 sierpnia 2026).
4. NASA, A study of the critical computational problems associated with strapdown inertial navigation systems, NASA-CR-968, https://ntrs.nasa.gov/api/citations/19680012183/downloads/19680012183.pdf (dostęp: 15 sierpnia 2026).
5. PX4, Using PX4's Navigation Filter (EKF2), https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf (dostęp: 15 sierpnia 2026).
6. PX4, GNSS-Degraded and Denied Flight — Dead-Reckoning Mode, https://docs.px4.io/main/en/advanced_config/gnss_degraded_or_denied_flight (dostęp: 15 sierpnia 2026).
7. NASA, Euler angles, quaternions, and transformation matrices — working relationships, https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (dostęp: 15 sierpnia 2026).
8. ROS, REP 145: Conventions for IMU Sensor Drivers, https://ros.org/reps/rep-0145.html (dostęp: 15 sierpnia 2026).

Powiązane tematy

IMU w droniePrzekrojowa inżynieria toru inercyjnego UAV: znaczenie pomiarów gyro i accel, modele błędów, zakresy, ODR, aliasing, timestamp, FIFO i DMA, filtry, drgania, montaż, kalibracja temperaturowa, redundancja, health monitoring oraz kwalifikacja laboratoryjna.Układy współrzędnych w UAVŁańcuch transformacji od współrzędnych geodezyjnych przez ECEF i lokalną płaszczyznę do body/sensor frame, z pułapkami wysokości, originu, osi i kowariancji.Filtr Kalmana i EKF w UAVPraktyczna anatomia filtra nawigacyjnego: model stanu i błędów, macierze Q/R, opóźnione obserwacje, bramkowanie innowacji, Joseph form i diagnostyka.GNSS w UAVOd pseudoodległości do rozwiązania RTK: integracja odbiornika, antena, opóźnienie, układy odniesienia i walidacja z innymi sensorami.Magnetometr w UAVMagnetometr jako trójosiowy miernik lokalnego pola: model Ziemi, kalibracja elipsoidy, zakłócenia prądowe i zasady bezpiecznego użycia w estymatorze.Barometr w UAVZależność ciśnienie–wysokość, pogoda i temperatura, błędy montażowe, tor cyfrowy sensora oraz fuzja barometru z IMU, GNSS i dalmierzem.Optical flow w UAVOptical flow jako pomiar kątowego ruchu obrazu: model projekcyjny, skala z dalmierza, kompensacja obrotu, rolling shutter, tekstura i oświetlenie, układy osi, timestamp, opóźnienie, kalibracja oraz walidacja w EKF.Dead reckoning UAVDead reckoning jako kontrolowany tryb systemowy: rozróżnienie czystej propagacji inercyjnej od aiding optical flow/VIO/airspeed, warunki wejścia i wyjścia, covariance i time-to-limit, continuity controller, source switching, odzyskanie GNSS bez skoku oraz plan testów dropout.

Źródła z centralnego rejestru

1. US Coast Guard Navigation Center: GPS User Equipment — Inertial Navigation Systems [podręcznik instytucji państwowej]
2. US Coast Guard Navigation Center: Augmented GPS — Inertial Navigation Systems [podręcznik instytucji państwowej]
3. NASA: Integrated INS/GPS Navigation from a Popular Perspective [publikacja techniczna]
4. NASA CR-968: A study of the critical computational problems associated with strapdown inertial navigation systems [raport techniczny]
5. PX4: Using PX4 Navigation Filter EKF2 [dokumentacja projektu]
6. PX4: GNSS-Degraded and Denied Flight — Dead-Reckoning Mode [dokumentacja projektu]
7. NASA: Euler angles, quaternions, and transformation matrices — working relationships [memorandum techniczne]
8. ROS REP 145: Conventions for IMU Sensor Drivers [standard projektu open source]
9. Sumit Sharma, „Drone Development from Concept to Flight” [książka]