Filtr Kalmana pro BIŇS a SNS v Engee

Optimální Kalmanův filtr pro komplexní navigaci BINS a SNS v Engee

Bezpilotní letoun (LA) využívá kombinaci bezpilotní inerciální navigační systému (BINS) a satelitní navigačního systému (SNS) k přesnému určení parametrů pohybu. Optimální Kalmanův filtr koriguje akumulované chyby BINS daty z SNS, což zajišťuje pozicní i rychlostní korekci. Implementace byla provedena v prostředí modelování Engee s diskrétní modelem s krokem 0,1 s.

BINS pracuje v rušeném režimu kvůli nepřesnostem gyroskopů, akcelerometrů, počátečních podmínek a výpočetního zařízení. Základní rovnice inerciální navigace zní: R' = V_n + g(R) + n, kde R je polohový vektor, V_n je zdánlivá rychlost, g je gradient tíhového pole.

SNS poskytuje přesnost 10–12 m v souřadnicích a 0,05 m/s ve rychlostech, v diferenciálním režimu až 1–2 m. Chyby vznikají z efemerid, zpoždění v ionosféře a troposféře, šumu přijímače.

Inicializace a parametry modelování

Simulace sleduje let na výšce 1000 m podél trasy s otočnými body: zeměpisné šířky [55°, 56,58°, 56,46°, 56,96°], zeměpisné délky [37°, 37,5°, 38°, 38,5°]. Konstanty: Rz = 6371000 m, g = 9,78049 m/s², dt = 0,1 s, t = 3600 s.

Počáteční chyby:

• λ_error = 6/Rz (délka)
• φ_error = 6/Rz (šířka)
• V_e_error = 0,05 m/s
• V_n_error = 0,05 m/s
• ψ_error = 0,25° (kurz)
• γ_error = 0,03° (naklonění)
• θ_error = 0,03° (naklopení)

Kód inicializace polí:

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);
# ... (další pole)

Otočné body:

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

Výpočet trajektorie po ortodromii

Ortodromie je nejkratší cesta na sféře. Pro jednotlivé úseky trasy se vypočítá:

• Změna délky: Δλ = λ_{j} - λ_{j-1}
• Délka ortodromie: σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• Vzdálenost: D = Rz ⋅ σ
• Kurz: ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

Normalizace ψ ∈ [0, 2π].

Celková vzdálenost sum(D), průměrná rychlost V = sum(D)/t.

Kód výpočtu kurzu:

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

Konstrukce trajektorie s ohledem na LUR

LUR (levý/pravý obrat): poloměr Rr = V²/(g tan γ), čas Tr = Rr ⋅ Δψ / V, dodatečná vzdálenost lur_dist = Rr tan(0,5 Δψ).

Projekce rychlostí: V_e = V sin ψ (východ), V_n = V cos ψ (sever). Úhlové rychlosti: w_E = V_n/Rz, w_N = V_e/Rz.

Přírůstky: dφ = w_E dt, dλ = (w_N / cos φ) dt.

Cyklus modelování přímých úseků a LUR zajišťuje spojitou trajektorii.

Diskrétní Kalmanův filtr

Dynamický model: X' = F X + G W, Z = H X + V.

Diskrétní forma:

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

Kde Φ je přechodová matice, Q/R jsou kovariance šumu.

Implementace v Engee s modelováním chyb

Cyklus filtrace imituje ukazatele gyroskopů a akcelerometrů s nulovým driftem, koeficientními chybami a šumem:

• 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 stejně

Kód 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-rozměrný stavový vektor X_out a ukládá trajektorie X_out_list, Pk_list.

Co je důležité

• Komplexní navigace BINS+SNS: Kalmanův filtr odstraňuje akumulaci chyb inerciálního systému pomocí dat ze satelitů.
• Ortodromická trajektorie: přesný výpočet LUR s poloměrem Rr = V²/(g tan γ).
• Modelování chyb: realistické drifty gyroskopů (0,001°/s) a akcelerometrů (60 μg).
• Diskrétní implementace: krok 0,1 s, přechodová matice Φ z lineárního modelu.
• Engee skripty: kompletní kódová báze pro reprodukci navigačního komplexu.

— Editorial Team