Model sensora w symulacji UAV

Model sensora nie powinien generować po prostu „wartości prawdziwej plus losowy szum”. Między stanem fizycznym symulatora a próbką odbieraną przez flight controller znajdują się: transformacja do punktu i osi czujnika, dynamika elementu pomiarowego, filtr analogowy i cyfrowy, zegar próbkowania, opóźnienie transportu, przeliczenie jednostek, kwantyzacja, ograniczenie zakresu, błędy kalibracji oraz status jakości. Pominięcie którejkolwiek z tych warstw może dać autopilotowi dane łatwiejsze niż w rzeczywistym statku.

Model należy budować do konkretnego celu. Do testu regulatora attitude wystarczy często IMU z poprawnym pasmem, opóźnieniem i clippingiem. Do oceny estymatora potrzebne są skorelowane błędy IMU, GNSS, barometru i magnetometru. Do testu sterownika magistrali konieczna jest jeszcze warstwa rejestrów, FIFO, przerwań i błędów transportu. Najbardziej złożony model nie jest automatycznie najlepszy; najlepszy jest model o jawnych założeniach, parametrach wyprowadzonych z pomiaru i walidacji odpowiadającej pytaniu testowemu.

Spis treści#

Łańcuch pomiarowy
Truth state nie jest próbką
Układy osi i punkt pomiaru
Czas i częstotliwość próbkowania
Pasmo i anti-aliasing
Opóźnienie, jitter i wiek danych
Model błędu
White noise i noise density
Bias i jego zmienność
Random walk i szum skorelowany
Skala, nieliniowość i cross-axis
Kwantyzacja, zakres i clipping
Dropout, stale value i status
Model IMU
Model GNSS
Barometr i airspeed
Magnetometr
Lidar, optical flow i kamera
Covariance i raportowana jakość
Parametry z datasheetu i logu
Allan deviation
Implementacja dyskretna
Gazebo i SDFormat
SITL, HIL i emulacja rejestrów
Deterministyczność
Walidacja modelu
Minimalny zestaw prób
Typowe błędy

Łańcuch pomiarowy#

Przydatny podział ma sześć warstw:

stan prawdziwy → geometria sensora → dynamika/pasmo → błędy pomiaru
              → sampling i timestamp → transport/API → flight controller

Stan prawdziwy obejmuje pozycję, orientację, prędkości, przyspieszenia, pole magnetyczne, ciśnienie i geometrię otoczenia. Geometria sensora ustala jego położenie względem środka masy oraz obrót osi. Dynamika opisuje pasmo, filtrację i czas odpowiedzi. Błędy pomiaru obejmują szum, bias, skalę, saturację i usterki. Sampling wybiera chwile obserwacji i przypisuje timestamp. Transport modeluje opakowanie w wiadomość, kolejkę, magistralę i status.

Każda warstwa powinna dać się osobno wyłączyć. Wtedy test A/B odpowiada na konkretne pytanie: czy niestabilność pochodzi z modelu mechaniki, opóźnienia IMU, aliasingu, czy zachowania estymatora przy błędnej jakości GNSS.

Truth state nie jest próbką#

Symulator fizyki zna wielkości niedostępne bezpośrednio w statku: idealną orientację, dokładną pozycję, prędkość w dowolnym punkcie i siły bez błędu pomiarowego. Te dane są ground truth i służą do oceny estymacji. Nie należy podawać ich na wejście algorytmu, który w realnym systemie otrzymuje surowy sensor.

Klasyczny błąd polega na przekazaniu do interfejsu „IMU” idealnej orientacji z silnika fizyki. Typowy surowy zestaw IMU dostarcza prędkość kątową oraz specific force, nie bezbłędny quaternion. Jeśli firmware ma testować własny filtr Kalmana, idealna orientacja omija zasadniczą część problemu. Oficjalny model danych ROS 2 także rozróżnia pola orientation, angular velocity i linear acceleration wraz z osobnymi macierzami covariance; brak estymaty orientacji można jawnie oznaczyć.[1]

Ground truth trzeba logować oddzielnie od pomiarów. Nazwy tematów, jednostki i typy powinny uniemożliwiać pomyłkę, na przykład truth/angular_velocity_body oraz sensor/imu0/angular_velocity.

Układy osi i punkt pomiaru#

Najpierw definiuje się co najmniej cztery ramy:

world/navigation, na przykład NED albo ENU;
body związane ze statkiem;
sensor związane z obudową konkretnego czujnika;
output frame wymagane przez protokół lub sterownik.

Macierz obrotu nie rozwiązuje całego problemu. Akcelerometr odsunięty od środka obrotu o wektor r mierzy dodatkowe składniki:

a_sensor = a_CG + α × r + ω × (ω × r)

Pierwszy dodatkowy człon pochodzi od przyspieszenia kątowego, drugi jest dośrodkowy. Dla małego multirotora odległość kilku centymetrów i wysoka prędkość kątowa mogą dać widoczny efekt. Model IMU umieszczony zawsze w środku masy nie ujawni błędu kompensacji lever arm.

Akcelerometr nie mierzy zwykłego przyspieszenia translacyjnego, lecz specific force. W spoczynku na stole wskazuje w osi pionowej wartość związaną z grawitacją, mimo że pozycja się nie zmienia. Znak zależy od konwencji osi. Test statyczny w sześciu orientacjach powinien wykazać oczekiwane ±g na kolejnych osiach i niemal zero na pozostałych.

Transformację warto testować niezależnie: obrót tylko wokół każdej osi, przyspieszenie tylko w jednej osi oraz czysty ruch obrotowy sensora odsuniętego od środka. Jedna zamiana NED/ENU lub body FRD/FLU potrafi wyglądać jak błąd regulatora.

Czas i częstotliwość próbkowania#

update_rate = 1000 Hz nie powinno oznaczać „wywołaj funkcję mniej więcej tysiąc razy na sekundę ścienną”. Model potrzebuje jawnego zegara symulacji i chwili pomiaru t_sample. Przy kroku fizyki Δt_phys i okresie sensora T_s należy akumulować czas lub planować następny deadline. Jeśli T_s nie jest wielokrotnością kroku fizyki, potrzebna jest interpolacja truth state albo kontrolowany błąd fazy.

Istotne są trzy czasy:

t_truth — chwila stanu użytego do obliczenia pomiaru;
t_sample — chwila wykonania konwersji;
t_arrival — chwila udostępnienia próbki odbiorcy.

Timestamp powinien reprezentować znaczenie określone przez interfejs. Jeśli wiadomość dotyczy chwili pomiaru, wstawianie czasu wysłania maskuje latency. Estymator z kompensacją opóźnienia potrzebuje wieku danych, nie momentu odebrania pakietu.

Sensory mają różne rate: IMU może pracować w setkach Hz lub kHz, magnetometr w dziesiątkach lub setkach Hz, GNSS zwykle znacznie wolniej. Publikowanie wszystkich na każdym kroku fizyki nadaje im sztuczną synchronizację i zerowy jitter. Z kolei duplikowanie ostatniej próbki bez flagi new data może skłonić firmware do wielokrotnego przetworzenia tej samej obserwacji.

Pasmo i anti-aliasing#

Sensor nie obserwuje nieskończonego pasma. Przed decymacją działają mechanika struktury, analogowy tor pomiarowy, cyfrowy low-pass i często wewnętrzny FIFO. Minimalny model pierwszego rzędu:

y[k] = y[k-1] + α(x[k] - y[k-1])
α = 1 - exp(-2π f_c Δt)

gdzie f_c jest częstotliwością odcięcia. Ten model nie zastępuje dokładnej charakterystyki układu, ale odwzorowuje skończony czas odpowiedzi i tłumienie wysokich częstotliwości.

Szum należy dodać na właściwym etapie. Jeśli datasheet podaje noise density przed wybranym filtrem, najpierw generuje się proces w odpowiednim paśmie, a potem filtruje. Dodanie niezależnego szumu już po filtrze może dać zbyt wysoką energię przy dużych częstotliwościach. Przy decymacji trzeba zastosować filtr anti-aliasing; samo wybieranie co N-tej próbki przenosi składowe powyżej Nyquista do pasma użytecznego.

Model regulatora powinien uwzględniać tę samą konfigurację filtra co realny sensor. Zmiana ODR, DLPF albo sposobu odczytu FIFO zmienia zarówno opóźnienie, jak i RMS szumu.

Opóźnienie, jitter i wiek danych#

Stałe opóźnienie można modelować kolejką posortowaną według czasu udostępnienia:

t_release = t_sample + latency

Jitter dodaje zmienność do latency albo okresu próbkowania. Nie powinien jednak losowo cofać czasu ani zmieniać kolejności próbek, chyba że test dotyczy właśnie reorderingu. Parametry powinny rozróżniać:

opóźnienie konwersji sensora;
oczekiwanie w FIFO;
czas transmisji magistrali;
scheduling sterownika;
opóźnienie transportu między symulatorem a FC.

Średnia nie wystarcza. Dwa interfejsy o średnim latency 2 ms mogą różnić się diametralnie: jeden ma stałe 2 ms, drugi 0 ms przez większość czasu i sporadyczne 20 ms. Dlatego raportuje się percentyle, maksimum, histogram i korelację z obciążeniem CPU.

W HIL opóźnienie ścienne i symulacyjne łatwo pomieszać. Jeżeli symulator zwalnia, ale timestamp nadal rośnie według czasu modelu, FC w trybie free-running może odebrać pakiety seriami. Stanowisko HIL musi ustalić autorytet czasu i regułę przeciążenia kolejki.

Model błędu#

Wygodny model wektorowego sensora ma postać:

z[k] = sat(Q((I + S + M) R x[k] + b[k] + n[k]))

gdzie:

x jest wartością prawdziwą;
R transformuje osie;
S opisuje błędy skali;
M opisuje nieortogonalność i cross-axis;
b jest biasem;
n jest szumem;
Q oznacza kwantyzację;
sat ogranicza wynik do zakresu.

Do tego dochodzą dynamika, timestamp, dropout i status. Parametrów nie należy dodawać bez kontroli identyfikowalności. Jeśli scale error i bias są jednocześnie dobierane dowolnie, wiele zestawów da podobny wynik w wąskim zakresie danych. Eksperyment kalibracyjny musi pobudzać osie i amplitudy pozwalające te składniki rozdzielić.

White noise i noise density#

Datasheety często podają noise density, na przykład w rad/s/√Hz, m/s²/√Hz, µT/√Hz albo Pa/√Hz. Nie jest to bezpośrednio odchylenie standardowe jednej próbki. Dla białego szumu ograniczonego pasmem B przybliżone RMS wynosi:

σ ≈ N_d √B

gdzie N_d to noise density. Efektywne pasmo szumowe filtra nie zawsze równa się jego częstotliwości -3 dB. Dla filtra pierwszego rzędu equivalent noise bandwidth różni się od prostego f_c, dlatego w dokładnym modelu wykorzystuje się ENBW toru.

Jeżeli model dyskretny generuje niezależny szum na każdej próbce, jego wariancja musi zależeć od rate i miejsca filtracji. W przeciwnym razie zmiana symulacji z 500 Hz na 1 kHz bez zmiany sensora niepoprawnie zmieni całkowitą energię szumu po filtrze.

Biały szum ma niemal zerową autokorelację poza lag zero i płaskie PSD w modelowanym paśmie. Sam histogram Gaussa nie dowodzi poprawności — próbki mogą mieć dobry rozkład amplitudy, ale złą korelację czasową.

Bias i jego zmienność#

Bias statyczny to offset obecny przez przebieg:

b[k] = b0

b0 może być stałą urządzenia, losowaniem per egzemplarz albo losowaniem per cold start. Te przypadki testują inne zjawiska. Losowanie nowego biasu w każdej próbce zamienia go w white noise i nie sprawdza kalibracji offsetu.

Realny bias może zależeć od temperatury, napięcia, czasu od startu, wibracji i obciążenia mechanicznego. Prosty model termiczny:

b(T) = b_ref + k1(T - T_ref) + k2(T - T_ref)^2

W praktyce temperatura sensora ma własną stałą czasową; użycie bezpośrednio temperatury otoczenia daje zbyt szybką zmianę. Jeśli firmware ma kompensację termiczną, model powinien generować też kanał temperatury z opóźnieniem i błędem.

Wolnozmienny bias można opisać procesem Gaussa-Markowa pierwszego rzędu. SDFormat przewiduje między innymi dynamic_bias_stddev oraz dynamic_bias_correlation_time dla wolnych zmian biasu.[2] To lepszy model testowy niż arbitralne dodanie sinusoidy, o ile parametry pochodzą z danych.

Random walk i szum skorelowany#

Random walk biasu w najprostszej dyskretyzacji:

b[k+1] = b[k] + σ_rw √Δt · ξ[k]

gdzie ξ ma rozkład normalny o zerowej średniej i wariancji jeden. Czynnik √Δt jest kluczowy: bez niego zmiana częstotliwości kroku zmienia fizyczną intensywność procesu.

Czysty random walk jest niestacjonarny i wariancja rośnie bez ograniczenia. Do wielu zastosowań lepszy jest proces Gaussa-Markowa:

b[k+1] = φ b[k] + q ξ[k]
φ = exp(-Δt/τ)

τ określa czas korelacji. Współczynnik q trzeba dobrać tak, aby wariancja stacjonarna odpowiadała założeniu. Nie należy mieszać σ stanu, density procesu i odchylenia pojedynczego kroku.

Szum między osiami może być skorelowany. Wtedy losuje się wektor u ~ N(0,I) i mnoży przez macierz L, dla której LLᵀ = C. Macierz covariance C musi być symetryczna i dodatnio półokreślona. Niezależne osie są użytecznym baseline, nie uniwersalną prawdą.

Skala, nieliniowość i cross-axis#

Błąd skali powoduje wynik proporcjonalny do sygnału. Dla jednej osi:

z = (1 + s)x + b

Jest niewidoczny przy x = 0, dlatego nie da się go wyznaczyć z samego logu spoczynkowego. Wektorowo błędy osi i nieortogonalność opisuje macierz bliska jednostkowej. Małe wyrazy poza przekątną powodują, że pobudzenie osi X pojawia się w Y i Z.

Nieliniowość może być modelowana wielomianem, tabelą LUT albo odcinkami. Histereza wymaga stanu, bo wynik zależy od kierunku dojścia. W typowym SITL nie trzeba odwzorowywać każdego efektu MEMS; warto je dodać, gdy test dotyczy kalibracji albo pracy blisko pełnej skali.

Misalignment mechaniczny sensora względem PCB i PCB względem body powinny być oddzielnymi transformacjami. Pierwszy jest cechą egzemplarza, drugi konfiguracją montażu. Dzięki temu test może odróżnić kalibrację fabryczną od parametru board rotation.

Kwantyzacja, zakres i clipping#

Kwantyzacja do kroku q:

z_q = q · round(z / q)

Warto ustalić regułę zaokrąglania i reprezentację liczb ujemnych. SDFormat obsługuje model gaussian_quantized oraz parametr precision.[2] Przy dużym szumie kwantyzacja może być mało widoczna; przy wolnym sygnale o wysokiej rozdzielczości staje się dominująca.

Zakres pełnej skali nie jest tylko limitem wartości. Po przekroczeniu może wystąpić:

clipping do wartości maksymalnej;
bit statusu overrange;
utrata próbki;
wrap przy błędnej konwersji typów;
czas powrotu po przeciążeniu.

Najbezpieczniejszy model domyślny to saturacja i jawna flaga. Wrap powinien być osobną usterką testową, bo zwykle oznacza błąd implementacji interfejsu, nie pożądane zachowanie sensora.

Clipping IMU podczas wibracji może być ważniejszy od RMS szumu. Model powinien zliczać próbki na szynie i logować maksima przed saturacją; inaczej analiza widzi „równy” sygnał o pozornie ograniczonej amplitudzie.

Dropout, stale value i status#

Brak pomiaru ma kilka różnych postaci:

pakiet nie powstaje;
pakiet jest opóźniony;
powtarzana jest poprzednia wartość z nowym timestampem;
powtarzana jest wartość ze starym timestampem;
wartość jest NaN albo sentinel;
dane są liczbowe, ale status mówi invalid;
urządzenie znika z magistrali.

Firmware może reagować na każdą inaczej. „Wyzeruj GNSS” nie jest odpowiednikiem utraty GNSS — zero może zostać potraktowane jak ważna pozycja geograficzna. Fault injection musi działać na warstwie odpowiadającej testowanemu wymaganiu.

Model stanu powinien umożliwiać sekwencję healthy → degraded → invalid → recovering → healthy. Powrót może wymagać czasu akwizycji, inicjalizacji filtra, kalibracji albo ponownego wykrycia urządzenia. Natychmiastowy idealny powrót po dropout daje zbyt optymistyczny test failsafe.

Model IMU#

Minimalny użyteczny model żyroskopu i akcelerometru obejmuje:

właściwe specific force i prędkość kątową w osi sensora;
offset punktu pomiaru;
ODR oraz timestamp próbki;
low-pass i grupowe opóźnienie;
white noise;
bias per start i wolną zmianę biasu;
scale/misalignment;
zakres, kwantyzację i clipping;
temperaturę, jeśli testowana jest kompensacja;
FIFO i opóźnienie batcha, jeśli firmware korzysta z FIFO.

Model wibracji nie powinien powstawać przez dodanie dowolnego szumu Gaussa. Wymuszenia od napędu mają linie zależne od RPM, częstotliwość blade-pass, harmoniczne, modulację i transmisję przez ramę. Do testu filtrów notch sensowniejsze jest nałożenie sygnału zsynchronizowanego z modelem silnika oraz osobnego broadband floor. Artykuł o luzach i rezonansach ramy opisuje rozdzielenie źródła, drogi transmisji i odpowiedzi.

Orientation z modelu IMU należy wyłączyć, jeśli realne urządzenie go nie dostarcza. Oficjalny tutorial Gazebo wskazuje, że IMU publikuje orientation, angular velocity i linear acceleration, a rate ustala update_rate; konfigurację trzeba więc świadomie dopasować do granicy testowanego systemu.[3]

Model GNSS#

Model GNSS nie może ograniczać się do niezależnego białego szumu latitude i longitude. Przydatne składniki to:

rate i latency rozwiązania;
korelacja błędów pozycji w czasie;
osobne błędy poziome i pionowe;
błąd prędkości i course;
liczba satelitów, fix type, DOP/accuracy i ich spójność;
lever arm anteny;
przerwy widoczności i reacquisition;
multipath zależny od sceny, jeśli cel testu tego wymaga;
wspólne i niezależne błędy odbiorników redundantnych.

Wolnozmienny błąd pozycji jest zwykle bardziej wymagający dla estymatora niż próbki niezależne, bo może wyglądać wiarygodnie przez dłuższy czas. Height nie powinien mieć automatycznie takiej samej wariancji jak horizontal. Prędkość nie może być zawsze idealną pochodną zaszumionej pozycji; odbiornik może wyznaczać ją inną metodą, a korelacje zależą od modelu.

Status musi być wewnętrznie spójny. Jeśli fix_type = no fix, nie należy jednocześnie podawać centymetrowej accuracy. Jeśli test dotyczy błędnej telemetrii statusu, taki przypadek powinien być nazwanym faultem, nie przypadkową konfiguracją.

Diagnostyka GNSS korzysta z innovations, residuals i porównania z innymi sensorami; symulacja powinna logować osobno prawdziwy błąd, zgłaszaną accuracy oraz faktyczny rozkład błędu.

Barometr i airspeed#

Barometr mierzy ciśnienie, a wysokość jest wielkością wyprowadzoną z modelu atmosfery. Model może liczyć ciśnienie z wysokości i warunków atmosferycznych, następnie dodawać bias, szum, temperaturę, opóźnienie oraz wpływ lokalnego przepływu. Bezpośrednie generowanie „wysokości barometrycznej” omija algorytm konwersji testowany w firmware.

Prop wash, ciśnienie dynamiczne przy otworze obudowy i nagrzanie sensora mogą powodować błąd zależny od throttle oraz prędkości. Prosty model empiryczny może używać tabeli biasu względem throttle/airspeed, ale powinien być zidentyfikowany z logu lub próby tunelowej, nie zgadywany.

Pitot mierzy różnicę ciśnień. Łańcuch obejmuje kierunek osi rurki, lokalny przepływ, gęstość powietrza, instalację przewodów, czas odpowiedzi, offset przetwornika i zakres. Przy małej prędkości kwantyzacja oraz offset mogą dominować; przy kącie natarcia i sideslip wskazanie nie musi odpowiadać modułowi prędkości względem powietrza. Model prędkościomierza i rurki Pitota powinien pozostać oddzielony od modelu wiatru.

Magnetometr#

Pole w osi sensora wynika z modelu pola Ziemi i orientacji, ale w UAV dochodzą zakłócenia platformy:

hard iron jako wektor offsetu;
soft iron jako macierz skali i nieortogonalności;
prądowe pole zależne od throttle/prądu;
przewody, magnesy silników i stalowe elementy;
zmiana z położeniem ruchomych części;
lokalne anomalie otoczenia.

Dodanie niezależnego szumu nie sprawdza kompensacji compass-motor. Do tego potrzebny jest sygnał skorelowany z prądem, najlepiej z opóźnieniem i ewentualną histerezą. Model dwóch magnetometrów powinien uwzględniać różne położenia i częściowo wspólne pole zewnętrzne.

W teście yaw należy sprawdzić pełny obrót w poziomie, pochylenia oraz zmianę throttle bez ruchu. Innowacja magnetometru powinna reagować na zakłócenie, a ground truth yaw pozostać oddzielny.

Lidar, optical flow i kamera#

Sensor odległości wymaga modelu wiązki i geometrii sceny. Poza Gaussian noise istotne są minimum/maximum range, brak odbicia, zależność od kąta, materiału, światła, fog/dust oraz wielodrogowość. Gazebo obsługuje różne typy sensorów, niestandardowe rate i Gaussian noise, ale wierność konkretnego zjawiska trzeba sprawdzić dla używanej wersji i pluginu.[4]

Optical flow zależy od tekstury, oświetlenia, ekspozycji, blur, wysokości, pola widzenia, ruchu obrotowego i jakości obrazu. Publikowanie idealnej prędkości XY jako „flow” omija właściwy algorytm. Jeśli test dotyczy tylko fuzji, dopuszczalny jest model abstrakcyjny, lecz musi generować jakość, saturację prędkości kątowej i błędy zależne od wysokości.

Kamera wymaga modelu intrinsics, distortion, rolling/global shutter, exposure, frame rate, latency, motion blur i drop frames. Render fotorealistyczny nie gwarantuje poprawnego czasu ekspozycji ani opóźnienia pipeline. Do testu nawigacji ważniejsza bywa zgodność geometrii i timestamp niż estetyka obrazu.

Covariance i raportowana jakość#

Covariance jest częścią obserwacji, a nie dekoracją. Jeżeli model generuje szum o wariancji σ², ale wiadomość stale zgłasza dziesięć razy mniejszą wartość, estymator będzie nadmiernie ufał pomiarowi. To może być celowy test niespójności, ale baseline powinien być zgodny.

Macierz covariance opisuje nie tylko wariancje osi na przekątnej, lecz także korelacje poza przekątną. W wielu interfejsach zero nie oznacza „brak szumu”; może oznaczać „covariance nieznana”. Definicja sensor_msgs/Imu nakazuje wypełnić znaną covariance, zera interpretuje jako nieznaną, a brak konkretnej estymaty sygnalizuje wartością -1 w pierwszym elemencie właściwej macierzy.[1]

Reported accuracy GNSS powinna być kalibrowana statystycznie: na przykład około 68% błędów jednowymiarowych mieści się w granicy 1σ tylko przy założeniach odpowiedniego rozkładu. Dla błędu dwuwymiarowego interpretacja promienia jest inna. Nie należy porównywać percentyla 95% z 1σ bez konwersji i jawnych założeń.

Parametry z datasheetu i logu#

Datasheet jest punktem startowym, nie kompletnym modelem. Trzeba zanotować warunki:

zakres i ODR;
konfigurację filtra;
temperaturę i napięcie;
typową albo maksymalną wartość;
jednostkę density/RMS/peak-to-peak;
czas uśredniania;
czy parametr dotyczy pojedynczej osi.

Nie wolno mieszać bias repeatability, in-run bias stability, zero-rate output, noise density i RMS noise. Nazwy bywają podobne, ale opisują inne eksperymenty i modele.

Z realnego urządzenia warto zebrać:

długi log statyczny przy stałej temperaturze;
cold start powtarzany wiele razy;
sweep temperatury z odpowiednim wygrzaniem;
sześć orientacji dla akcelerometru;
obrót referencyjny dla żyroskopu;
pobudzenie dynamiczne z referencją czasu;
log timestampów, FIFO, drop i temperatury.

Średnia oraz standard deviation nie wystarczą. Analizuje się PSD, autokorelację, histogram, outliers, zależność od temperatury i stabilność w kolejnych oknach.

Allan deviation#

Allan variance/deviation jest użyteczna do charakteryzacji procesów o różnych skalach czasu, zwłaszcza gdy klasyczna wariancja słabo opisuje proces niestacjonarny albo długopamięciowy. NIST przedstawia statystyczne podstawy tej miary dla procesów stacjonarnych, random walk i procesów 1/f.[5]

Dla równomiernie próbkowanego sygnału tworzy się średnie w klastrach o czasie τ, a następnie różnice sąsiednich średnich. Wykres log-log Allan deviation względem τ pomaga rozdzielać obszary zdominowane przez white noise, bias instability i random walk. Nie wystarczy jednak odczytać jednego punktu i wstawić go bezpośrednio jako σ pojedynczego kroku.

Procedura powinna zapisać:

surowy rate i czas trwania logu;
usunięte próbki/outliers;
zakres τ z wystarczającą liczbą klastrów;
metodę detrendingu;
przedziały ufności;
mapowanie parametrów wykresu na równania modelu.

Walidacja polega na wygenerowaniu długiego logu z modelu i porównaniu jego Allan deviation z urządzeniem. Zgodny pojedynczy RMS przy 1 s nie gwarantuje zgodności w 0,01 s i 100 s.

Implementacja dyskretna#

Poniższy pseudokod pokazuje kolejność, nie gotowy sterownik:

on_physics_step(t, truth):
    while next_sample_time <= t:
        x = interpolate_truth(next_sample_time)
        x = transform_to_sensor_frame(x, pose_sensor)
        x = analog_dynamics.update(x, sample_dt)

        bias = bias_model.update(sample_dt, temperature)
        noise = correlated_noise.sample(sample_dt)
        z = calibration_matrix * x + bias + noise
        z = quantize(saturate(z, full_scale), lsb)

        packet = encode(z, timestamp=next_sample_time, status=health)
        release_queue.push(next_sample_time + latency.sample(), packet)
        next_sample_time += sample_period_with_clock_error()

on_transport_step(t):
    release all packets with release_time <= t according to fault policy

Filtrację, szum i kwantyzację można przestawić tylko wtedy, gdy odpowiada to realnemu torowi. Na przykład kwantyzacja przed cyfrowym filtrem i po nim daje inne wyniki.

Generator powinien przechowywać seed, parametry i wersję modelu w logu. Stan biasu oraz filtra musi być resetowany według jawnej reguły: soft reset FC niekoniecznie oznacza cold start sensora.

Gazebo i SDFormat#

W Gazebo sensor jest zwykle elementem przypisanym do linku, a update_rate kontroluje częstotliwość generowania danych. Oficjalny tutorial Harmonic pokazuje konfigurację IMU, contact i lidar oraz odczyt tematów gz.[3] SDFormat definiuje pola noise, między innymi mean, stddev, bias_mean, bias_stddev, dynamic bias i quantization precision.[2]

Przykład fragmentu SDF:

<sensor name="imu0" type="imu">
  <always_on>true</always_on>
  <update_rate>1000</update_rate>
  <imu>
    <angular_velocity>
      <x>
        <noise type="gaussian_quantized">
          <mean>0</mean>
          <stddev>0.0002</stddev>
          <bias_mean>0</bias_mean>
          <bias_stddev>0.00001</bias_stddev>
          <dynamic_bias_stddev>0.000002</dynamic_bias_stddev>
          <dynamic_bias_correlation_time>600</dynamic_bias_correlation_time>
          <precision>0.000001</precision>
        </noise>
      </x>
    </angular_velocity>
  </imu>
</sensor>

Liczby są wyłącznie przykładem składni. Parametry produkcyjne należy wyprowadzić z datasheetu i pomiaru. Trzeba też sprawdzić, czy używana wersja gz-sensors implementuje wszystkie pola dla danego typu sensora. Dokumentacja Gazebo Classic ostrzega, że sensory mogą być domyślnie idealne i szum trzeba dodać jawnie; opisuje też osobne noise i bias IMU.[6] Aktualna implementacja powinna być weryfikowana w dokumentacji oraz wersji przypiętej przez projekt.

SITL, HIL i emulacja rejestrów#

Ten sam profil sensora można realizować na różnych granicach:

Granica	Co podaje model	Co jest prawdziwe	Typowe zastosowanie
algorytmiczna	wektor pomiaru	estymator/regulator	szybkie testy jednostkowe
middleware	wiadomość uORB/ROS/MAVLink	scheduling i aplikacja	SITL/integracja
HIL	pakiet sensorów po łączu	MCU i firmware	timing, CPU, watchdog
bus emulation	rejestry/FIFO/IRQ SPI/I²C	driver i cały firmware	sterownik sensora
sensor stimulation	fizyczne pobudzenie	sensor, driver, firmware	kwalifikacja toru end-to-end

Model wiadomości nie wykryje błędu burst read SPI ani złego kasowania przerwania FIFO. Emulacja rejestrów nie wykryje mechanicznego rezonansu obudowy. Każda warstwa odpowiada na inne pytanie i nie należy nazywać ich zamiennie „pełnym HIL”.

W PX4 SIH model fizyki publikuje symulowane IMU, GNSS, barometr, magnetometr i airspeed przez uORB, a odczytuje wyjścia aktuatorów; praca lockstep sprzyja powtarzalności.[7] To dobry przykład granicy middleware, lecz nie zastępuje emulacji elektrycznego interfejsu konkretnego układu.

Deterministyczność#

Losowość testu musi być kontrolowana. Minimalny zapis przebiegu obejmuje:

master seed;
osobne strumienie RNG dla sensorów i typów błędu;
wersję kodu i zestaw parametrów;
krok fizyki i rate sensorów;
scenariusz fault injection;
stan początkowy biasów;
identyfikator świata i modelu.

Osobne strumienie są ważne: dodanie kamery nie powinno zmieniać sekwencji szumu IMU przez zużycie liczb z jednego globalnego RNG. Deterministyczny baseline służy debugowaniu. Oddzielna kampania wielu seedów mierzy odporność statystyczną.

Replay samych sensorów eliminuje zależność aktuator–fizyka i jest testem open-loop. Replay pełnego przebiegu może być użyteczny dla estymatora, ale nie dowodzi stabilności zamkniętej pętli.

Walidacja modelu#

Walidację dzieli się na trzy poziomy:

Weryfikacja implementacji odpowiada, czy kod realizuje równania: test znanego wektora osi, dokładnego biasu, saturacji, okresu i kolejki latency.

Walidacja składników porównuje statystyki z urządzeniem: mean, variance, PSD, autokorelację, Allan deviation, rozkład cold-start biasu, response step i delay.

Walidacja zastosowania sprawdza, czy model wystarcza do decyzji: czy ranking konfiguracji filtra, margines stabilności albo reakcja failsafe zgadzają się z próbą stanowiskową/lotną.

Nie istnieje jeden wynik „model prawidłowy”. Model może być poprawny dla regulatora 20 Hz i błędny dla analizy wibracji 1 kHz. Raport powinien podawać zakres amplitudy, częstotliwości, temperatury, dynamiki i scenariuszy, w których model zwalidowano.

Porównanie wykresów „na oko” nie wystarcza. Definiuje się metryki przed próbą, na przykład:

błąd średniego biasu;
stosunek PSD w określonych pasmach;
błąd percentyla 95 latency;
zgodność clipping count;
błąd slope/poziomu Allan deviation w zakresach τ;
innovation consistency estymatora;
odsetek przebiegów spełniających kryterium misji.

Minimalny zestaw prób#

Test 1 — geometria i jednostki#

Wygeneruj spoczynek w sześciu orientacjach, obroty jednosiowe oraz znane przyspieszenie. Sprawdź znaki, g, rad/s zamiast deg/s i transformację sensor–body.

Test 2 — rate i czas#

Uruchom co najmniej 60 s czasu symulacji. Policz interwały, brakujące/zdublowane sequence ID, różnicę timestamp–arrival oraz zachowanie przy faster-than-real-time.

Test 3 — charakterystyka częstotliwościowa#

Podaj sweep sinusoidalny. Zmierz amplitude ratio i phase lag, porównaj z założonym filtrem. Powtórz blisko Nyquista i po decymacji.

Test 4 — statystyka statyczna#

Zapisz długi log bez ruchu. Porównaj mean, RMS, histogram, PSD, autokorelację i Allan deviation z profilem docelowym.

Test 5 — zakres#

Przekrocz pełną skalę dodatnią i ujemną. Zweryfikuj saturację, flagę, licznik clippingu i czas powrotu.

Test 6 — usterki#

Wstrzyknij dropout, stale value ze starym i nowym timestampem, duży latency, bias ramp, NaN/status invalid i reset sensora. Sprawdź reakcję estymatora oraz maszyny stanów failsafe.

Test 7 — wielosensorowość#

Uruchom dwa sensory z błędem wspólnym i niezależnym. Zweryfikuj voting, przełączenie źródła, korelację i brak fałszywego przekonania, że redundancja usuwa common-cause failure.

Typowe błędy#

Dodanie normal(0, σ) bez określenia rate, bandwidth i jednostki.
Losowanie nowego biasu w każdej próbce.
Publikowanie idealnego quaternionu jako surowej IMU.
Użycie przyspieszenia środka masy bez specific force i lever arm.
Jeden timestamp równy momentowi wysłania dla wszystkich sensorów.
Brak anti-aliasing przed decymacją.
GNSS z niezależnym szumem pozycji w każdej epoce i idealną prędkością.
Covariance równa zero mimo znanego modelu albo niezgodna z faktycznym błędem.
Natychmiastowy powrót sensora do idealnej pracy po dropout.
Jeden globalny RNG zmieniający wszystkie przebiegi po dodaniu nowej funkcji.
Parametry „typowe” z datasheetu prezentowane jako granice gwarantowane.
Walidacja tylko histogramu bez PSD, korelacji i czasu.

Dobry model sensora jest mierzalnym kontraktem. Określa co sensor obserwuje, kiedy obserwuje, w jakich osiach i jednostkach, z jaką dynamiką, błędem oraz informacją o jakości. Dopiero taki kontrakt pozwala sensownie ocenić estymator, regulator i failsafe, a później wskazać, które rozbieżności między SITL, HIL i próbą fizyczną wynikają z sensora, a które z modelu statku.

Powiązane tematy#

Przypisy#

Open Robotics, ROS 2 sensor_msgs/Imu message definition — jednostki, pola orientation/angular velocity/linear acceleration i semantyka covariance.
Open Source Robotics Foundation, SDFormat 1.12 sensor specification — definicje update_rate, Gaussian noise, bias, dynamic bias i quantization precision.
Open Robotics, Gazebo Harmonic: Sensors tutorial — konfiguracja sensorów, IMU oraz znaczenie update_rate i tematów danych.
Open Robotics, Gazebo Harmonic feature comparison i biblioteka gz-sensors — zakres sensorów i funkcji modelowania; wsparcie należy sprawdzać dla wersji przypiętej przez projekt.
N. F. Zhang, NIST, Allan Variance of Time Series Models of Measurement Data, Metrologia, 2008.
Open Robotics, Gazebo Sensor Noise Model — Gaussian noise oraz rozróżnienie szumu i biasu IMU; materiał opisuje Gazebo Classic, dlatego składnię i implementację trzeba zweryfikować dla aktualnego Gazebo.
PX4 Project, SIH Simulation — sensory publikowane przez uORB i deterministyczna praca lockstep.
MAVLink, Common Message Set — pola i jednostki wiadomości HIL_SENSOR, HIL_GPS oraz sygnalizacja aktualizowanych pól.

Źródła z centralnego rejestru

Gazebo Harmonic: Sensors tutorial [oficjalna dokumentacja symulatora]
Gazebo: Sensor Noise Model [oficjalna dokumentacja symulatora; dotyczy Gazebo Classic, zasady modelowania pozostają użyteczne]
SDFormat 1.12: sensor and noise model specification [specyfikacja formatu modelu symulacyjnego]
PX4 Simulation [dokumentacja]
PX4 Guide: SIH Simulation [dokumentacja projektu open source]
MAVLink Common Message Set: HIL_SENSOR, HIL_GPS, HIL_STATE_QUATERNION and HIL_ACTUATOR_CONTROLS [specyfikacja protokołu]
NIST: Allan Variance of Time Series Models of Measurement Data [publikacja metrologiczna NIST]
ROS 2 sensor_msgs/Imu message definition [oficjalna dokumentacja modelu danych ROS 2]
Sumit Sharma, „Drone Development from Concept to Flight” [książka]