Kalman-Filter für INS und GNSS in Engee

Optimaler Kalman-Filter für INS/GNSS-Integration in Engee

Unbemannte Luftfahrzeuge (UAVs) setzen auf eine Kombination aus inertialen Navigationssystemen (INS) und Satellitennavigationssystemen (GNSS), um Bewegungsparameter präzise zu bestimmen. Der optimale Kalman-Filter korrigiert akkumulierte Fehler im INS mithilfe von GNSS-Daten und liefert genaue Positions- und Geschwindigkeitsupdates. Die Implementierung erfolgt im Simulationsumfeld Engee mit einem diskreten Modellschritt von 0,1 Sekunden.

Das INS arbeitet unter gestörten Bedingungen aufgrund von Ungenauigkeiten bei Gyroskopen und Beschleunigungsmessern, Fehlern der Anfangsbedingungen sowie Rechenverzögerung. Die zentrale Gleichung der Inertialnavigation lautet R' = V_n + g(R) + n, wobei R der Positionsvektor, V_n die scheinbare Geschwindigkeit und g der Schweregradient ist.

GNSS bietet Positionsgenauigkeit von 10 bis 12 Metern und Geschwindigkeitsgenauigkeit von 0,05 m/s, steigert sich im differenziellen Modus auf 1 bis 2 Meter. Ursachen für Fehler sind ungenaue Ephemeriden, Ionosphären- und Troposphärenverzögerungen sowie Empfänger-Rauschen.

Initialisierung und Simulationsparameter

Die Flugbahn wird in einer Höhe von 1.000 Metern entlang einer Route mit Wendepunkten simuliert: Breiten [55°, 56,58°, 56,46°, 56,96°], Längen [37°, 37,5°, 38°, 38,5°]. Verwendete Konstanten: Rz = 6.371.000 m, g = 9,78049 m/s², dt = 0,1 s, t = 3.600 s.

Anfangsfehler:

• λ_error = 6/Rz (Länge)
• φ_error = 6/Rz (Breite)
• V_e_error = 0,05 m/s
• V_n_error = 0,05 m/s
• ψ_error = 0,25° (Kurs)
• γ_error = 0,03° (Seitenneigung)
• θ_error = 0,03° (Nick)

Initialisierungscode für Arrays:

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);
# ... (andere Arrays)

Wendepunkte:

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

Bahnberechnung mittels Großkreisnavigation

Die Großkreisnavigation definiert den kürzesten Weg auf einer Kugel. Für jeden Abschnitt der Route werden folgende Werte berechnet:

• Längenänderung: Δλ = λ_{j} - λ_{j-1}
• Großkreisabstand: σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• Distanz: D = Rz ⋅ σ
• Kurs: ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

Der Kurs ψ wird auf den Bereich [0, 2π] normiert.

Gesamtdistanz sum(D), Durchschnittsgeschwindigkeit V = sum(D)/t.

Code zur Kursberechnung:

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

Bahnkonstruktion mit Links-/Rechtskurven (LUR)

LUR (Links-/Rechtskurve): Radius Rr = V²/(g tan γ), Zeit Tr = Rr ⋅ Δψ / V, zusätzliche Distanz lur_dist = Rr tan(0,5 Δψ).

Geschwindigkeitsprojektionen: V_e = V sin ψ (Ost), V_n = V cos ψ (Nord). Winkelgeschwindigkeiten: w_E = V_n/Rz, w_N = V_e/Rz.

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

Die Simulations-Schleife für gerade Strecken und LUR stellt eine kontinuierliche Bahn sicher.

Diskreter Kalman-Filter

Dynamisches Modell: X' = F X + G W, Z = H X + V.

Diskrete Form:

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

Dabei ist Φ die Zustandsübergangsmatrix, Q und R die Kovarianzen von Prozess- und Messrauschen.

Implementierung in Engee mit Fehlermodellierung

Die Filter-Schleife simuliert Sensorwerte von Gyroskopen und Beschleunigungsmessern mit Nullpunktverschiebung, Skalierungsfehlern und zufälligem Rauschen:

• Gyroskope: zero_error = 0,001°/s, koef_error = 2×10⁻⁵, noise = 0,01×zero_error×randn()
• Beschleunigungsmesser: zero_error = g×60×10⁻⁶, koef_error = 3×10⁻⁶, noise ähnlich

Fragment-Code:

for i=1:1:length(lam)-1
    i=i+1;
    # Gyroskope
    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;
    # Beschleunigungsmesser
    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

Der Filter aktualisiert einen 19-dimensionalen Zustandsvektor X_out, speichert Bahnen in X_out_list und Kovarianzmatrizen in Pk_list.

Wichtige Erkenntnisse

• INS/GNSS-Integration: Der Kalman-Filter beseitigt die Akkumulation von Drift im inertialen System durch Nutzung von Satellitendaten.
• Großkreis-Navigation: Präzise Berechnung von LUR mit Radius Rr = V²/(g tan γ).
• Fehlermodellierung: Realistische Gyroskopdrift (0,001°/s) und Beschleunigungsmesserverzerrung (60 μg).
• Diskrete Umsetzung: Schrittweite von 0,1 Sekunden, Zustandsübergangsmatrix Φ aus linearisierten Dynamiken abgeleitet.
• Engee-Skripte: Vollständiger Codebase zur Nachbildung des integrierten Navigationssystems.

— Editorial Team