Filtr Kalmana dla BINS i SNS w Engee

Optymalny filtr Kalmana do kompensacji BINS i SNS w Engee

Bezpilotowy statek powietrzny (SP) wykorzystuje połączenie bezinercyjnego systemu nawigacyjnego inercyjnego (BINS) i satelitarnego systemu nawigacyjnego (SNS), aby precyzyjnie określać parametry ruchu. Optymalny filtr Kalmana koryguje akumulowane błędy BINS na podstawie danych SNS, zapewniając korektę pozycji i prędkości. Implementacja została wykonana w środowisku symulacji Engee z dyskretnym modelem o kroku 0,1 s.

BINS działa w warunkach zakłóceń spowodowanych błędami gyroskopów, akcelerometrów, warunkami początkowymi oraz obliczeniami. Podstawowe równanie nawigacji inercyjnej to: R' = V_n + g(R) + n, gdzie R to wektor położenia, V_n to prędkość pozorna, g to gradient siły grawitacji.

SNS zapewnia dokładność 10–12 m w położeniu i 0,05 m/s w prędkości, w trybie różnicowym nawet do 1–2 m. Błędy pochodzą z ephemerid, opóźnień w jonosferze i troposferze oraz szumów odbiornika.

Inicjalizacja i parametry symulacji

Symulowany lot na wysokości 1000 m trasą z punktami skrętu: szerokości [55°, 56,58°, 56,46°, 56,96°], długości [37°, 37,5°, 38°, 38,5°]. Stałe wartości: Rz = 6371000 m, g = 9,78049 m/s², dt = 0,1 s, t = 3600 s.

Początkowe błędy:

• λ_error = 6/Rz (długość)
• φ_error = 6/Rz (szerokość)
• V_e_error = 0,05 m/s
• V_n_error = 0,05 m/s
• ψ_error = 0,25° (kurs)
• γ_error = 0,03° (chył)
• θ_error = 0,03° (nachylenie)

Kod inicjalizacji tablic:

X_out=zeros(19,1);
w = zeros(19,1);
sigma = zeros(19,1);
D = zeros(19,1);
psi = zeros(19,1);
Fi = zeros(36001,1);
lam = zeros(36001,1);
# ... (reszta tablic)

Punkty skrętu:

fi_o = [55, 56.58, 56.46, 56.96].*pi/180;
lam_o = [37, 37.5, 38, 38.5].*pi/180;
H = 1000;

Obliczanie trajektorii po ortodromii

Ortodromia to najkrótsza droga na sferze. Dla odcinków trasy oblicza się:

• Zmiana długości: Δλ = λ_{j} - λ_{j-1}
• Długość ortodromii: σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• Odległość: D = Rz ⋅ σ
• Kurs: ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

Normalizacja ψ ∈ [0, 2π].

Całkowita odległość sum(D), średnia prędkość V = sum(D)/t.

Kod obliczania kursu:

for j = 2:1:4
    w[j-1]=lam_o[j]-lam_o[j-1];
    sigma[j-1]=acos(sin(fi_o[j-1])*sin(fi_o[j])+cos(fi_o[j-1])*cos(fi_o[j])*cos(w[j-1]));
    D[j-1]=Rz*sigma[j-1];
    psi[j-1]=atan(cos(fi_o[j])*sin(w[j-1]),cos(fi_o[j-1])*sin(fi_o[j])-sin(fi_o[j-1])*cos(fi_o[j])*cos(w[j-1]));
    if (psi[j-1] <= 0) psi[j-1] = psi[j-1] + 2*pi; end
    if (psi[j-1] > 2*pi) psi[j-1] = psi[j-1] - 2*pi; end
end

Budowanie trajektorii z uwzględnieniem skrętów

Skręty (LUR – lewy/prawy obrót): promień Rr = V²/(g tan γ), czas Tr = Rr ⋅ Δψ / V, dodatkowa odległość lur_dist = Rr tan(0,5 Δψ).

Projekcje prędkości: V_e = V sin ψ (wschód), V_n = V cos ψ (północ). Prędkości kątowe: w_E = V_n/Rz, w_N = V_e/Rz.

Przyrosty: dφ = w_E dt, dλ = (w_N / cos φ) dt.

Pętla symulacji odcinków prostoliniowych i skrętów zapewnia ciągłą trajektorię.

Dyskretny filtr Kalmana

Model dynamiczny: X' = F X + G W, Z = H X + V.

Postać dyskretna:

X̃_k = Φ X_{k-1} + Γ w_{k-1}

P̃_k = Φ P_{k-1} Φᵀ + Q

K_k = P̃_k Hᵀ (H P̃_k Hᵀ + R)^{-1}

X_k = X̃_k + K_k (Z_k - H X̃_k)

P_k = (I - K_k H) P̃_k

Gdzie Φ to macierz przejścia, Q/R to kowariancje szumów.

Realizacja w Engee z modelowaniem błędów

Pętla filtra symuluje dane gyroskopów i akcelerometrów z zerowym dryfem, błędami współczynnikowymi i szumem:

• Gyroskopy: zero_error = 0,001°/s, koef_error = 2×10⁻⁵, noise = 0,01×zero_error×randn()
• Akcelerometry: zero_error = g×60×10⁻⁶, koef_error = 3×10⁻⁶, noise analogicznie

Kod fragmentu:

for i=1:1:length(lam)-1
    i=i+1;
    # gyroskopy
    x_gyro_zero_error=0.001*pi/180;
    x_gyro_noise=x_gyro_zero_error*0.01*randn(1);
    x_gyro_koef_error=2*10^-5;
    # akcelerometry
    x_ac_zero_error=g*60*(10^-6);
    x_ac_noise=x_ac_zero_error*0.01*randn(1);
    x_ac_koef_error=3*10^-6;
    # ...
end

Filtr aktualizuje 19-wymiarowy wektor stanu X_out, zapisując trajektorie X_out_list, Pk_list.

Co jest ważne

• Kompensacja BINS + SNS: filtr Kalmana eliminuje akumulację błędów systemu inercyjnego za pomocą danych satelitarnych.
• Trajektoria ortodromiczna: precyzyjne obliczanie skrętów z promieniem Rr = V²/(g tan γ).
• Modelowanie błędów: realistyczne dryfty gyroskopów (0,001°/s) i akcelerometrów (60 μg).
• Realizacja dyskretna: krok 0,1 s, macierz przejścia Φ z liniowego modelu.
• Skrypty Engee: pełna baza kodu do odtworzenia układu nawigacyjnego.

— Editorial Team