Filtre de Kalman pour INS et GNSS dans Engee

Filtre de Kalman optimal pour l'intégration INS/GNSS dans Engee

Les véhicules aériens sans pilote (UAV) dépendent d'une combinaison de systèmes de navigation inertielle (INS) et de systèmes de navigation par satellite (GNSS) pour déterminer précisément les paramètres du mouvement. Le filtre de Kalman optimal corrige les erreurs accumulées par l'INS à l'aide des données GNSS, offrant des mises à jour précises de position et de vitesse. L'implémentation est réalisée dans l'environnement de simulation Engee avec un pas de modèle discret de 0,1 seconde.

L'INS fonctionne dans des conditions perturbées en raison d'inexactitudes des gyroscopes et des accéléromètres, d'erreurs initiales et de dérive computationnelle. L'équation fondamentale de la navigation inertielle est R' = V_n + g(R) + n, où R est le vecteur de position, V_n est la vitesse apparente, g est le gradient de gravité.

Le GNSS fournit une précision de positionnement de 10 à 12 mètres et une précision de vitesse de 0,05 m/s, s'améliorant à 1 à 2 mètres en mode différentiel. Les erreurs proviennent d'inexactitudes des éphémérides, de retards ionosphériques et troposphériques, ainsi que du bruit du récepteur.

Initialisation et paramètres de simulation

Le vol est simulé à une altitude de 1 000 mètres suivant une trajectoire avec des points de virage : latitudes [55°, 56,58°, 56,46°, 56,96°], longitudes [37°, 37,5°, 38°, 38,5°]. Constantes utilisées : Rz = 6 371 000 m, g = 9,78049 m/s², dt = 0,1 s, t = 3 600 s.

Erreurs initiales :

• λ_error = 6/Rz (longitude)
• φ_error = 6/Rz (latitude)
• V_e_error = 0,05 m/s
• V_n_error = 0,05 m/s
• ψ_error = 0,25° (cap)
• γ_error = 0,03° (roulis)
• θ_error = 0,03° (tangage)

Code d'initialisation des tableaux :

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);
# ... (autres tableaux)

Points de virage :

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

Calcul de trajectoire par navigation sur grand cercle

La navigation sur grand cercle définit le chemin le plus court sur une sphère. Pour chaque segment de la trajectoire, les éléments suivants sont calculés :

• Variation de longitude : Δλ = λ_{j} - λ_{j-1}
• Distance sur grand cercle : σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• Distance : D = Rz ⋅ σ
• Cap : ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

Le cap ψ est normalisé dans l'intervalle [0, 2π].

Distance totale : sum(D), vitesse moyenne V = sum(D)/t.

Code de calcul du cap :

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

Construction de trajectoire avec virages gauche/droite (LUR)

LUR (virage gauche/droite) : rayon Rr = V²/(g tan γ), temps Tr = Rr ⋅ Δψ / V, distance supplémentaire lur_dist = Rr tan(0,5 Δψ).

Projections de vitesse : V_e = V sin ψ (est), V_n = V cos ψ (nord). Vitesses angulaires : w_E = V_n/Rz, w_N = V_e/Rz.

Incréments : dφ = w_E dt, dλ = (w_N / cos φ) dt.

La boucle de simulation pour les segments droits et les LUR garantit une trajectoire continue.

Filtre de Kalman discret

Modèle dynamique : X' = F X + G W, Z = H X + V.

Forme discrète :

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

Où Φ est la matrice de transition d'état, et Q/R représentent les covariances du bruit de processus et de mesure.

Implémentation dans Engee avec modélisation des erreurs

La boucle de filtrage simule les mesures des gyroscopes et des accéléromètres avec un biais nul, des erreurs de facteur d'échelle et un bruit aléatoire :

• Gyroscopes : zero_error = 0,001°/s, koef_error = 2×10⁻⁵, noise = 0,01×zero_error×randn()
• Accéléromètres : zero_error = g×60×10⁻⁶, koef_error = 3×10⁻⁶, noise similaire

Extrait de code :

for i=1:1:length(lam)-1
    i=i+1;
    # gyroscopes
    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;
    # accéléromètres
    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

Le filtre met à jour un vecteur d'état de dimension 19 X_out, stocke les trajectoires dans X_out_list et les matrices de covariance dans Pk_list.

Points clés

• Intégration INS/GNSS : Le filtre de Kalman élimine l'accumulation de dérive dans les systèmes inertiels grâce aux données satellitaires.
• Navigation sur grand cercle : Calculs précis de LUR avec rayon Rr = V²/(g tan γ).
• Modélisation des erreurs : Dérive réaliste des gyroscopes (0,001°/s) et biais des accéléromètres (60 μg).
• Implémentation discrète : Pas de 0,1 seconde, matrice de transition d'état Φ dérivée des dynamiques linéarisées.
• Scripts Engee : Code complet pour reproduire le système de navigation intégré.

— Editorial Team