Filtro de Kalman para INS y GNSS en Engee

Filtro de Kalman óptimo para la integración INS/GNSS en Engee

Los vehículos aéreos no tripulados (UAVs) dependen de una combinación de sistemas de navegación inercial (INS) y sistemas de navegación por satélite (GNSS) para determinar con precisión los parámetros de movimiento. El filtro de Kalman óptimo corrige los errores acumulados en el INS utilizando datos del GNSS, proporcionando actualizaciones precisas de posición y velocidad. La implementación se realiza en el entorno de simulación Engee con un paso de modelo discreto de 0,1 segundos.

El INS opera bajo condiciones perturbadas debido a inexactitudes en los giroscopios y acelerómetros, errores en las condiciones iniciales y deriva computacional. La ecuación fundamental de la navegación inercial es R' = V_n + g(R) + n, donde R es el vector de posición, V_n es la velocidad aparente y g es el gradiente gravitatorio.

El GNSS ofrece una precisión de posicionamiento de 10–12 metros y una precisión de velocidad de 0,05 m/s, mejorando hasta 1–2 metros en modo diferencial. Los errores provienen de inexactitudes en las efemérides, retrasos ionosféricos y troposféricos, y ruido del receptor.

Inicialización y parámetros de simulación

La trayectoria se simula a una altitud de 1.000 metros siguiendo una ruta con puntos de giro: latitudes [55°, 56,58°, 56,46°, 56,96°], longitudes [37°, 37,5°, 38°, 38,5°]. Se utilizan constantes: Rz = 6.371.000 m, g = 9,78049 m/s², dt = 0,1 s, t = 3.600 s.

Errores iniciales:

• λ_error = 6/Rz (longitud)
• φ_error = 6/Rz (latitud)
• V_e_error = 0,05 m/s
• V_n_error = 0,05 m/s
• ψ_error = 0,25° (rumbo)
• γ_error = 0,03° (banqueo)
• θ_error = 0,03° (pique)

Código de inicialización para arreglos:

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);
# ... (otros arreglos)

Puntos de giro:

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

Cálculo de trayectoria mediante navegación por círculo máximo

La navegación por círculo máximo define la ruta más corta sobre una esfera. Para cada segmento de la ruta se calculan:

• Cambio de longitud: Δλ = λ_{j} - λ_{j-1}
• Distancia del círculo máximo: σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• Distancia: D = Rz ⋅ σ
• Rumbo: ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

El rumbo ψ se normaliza al rango [0, 2π].

Distancia total sum(D), velocidad promedio V = sum(D)/t.

Código para el cálculo del rumbo:

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

Construcción de trayectoria con giros izquierda/derecha (LUR)

LUR (giro izquierda/derecha): radio Rr = V²/(g tan γ), tiempo Tr = Rr ⋅ Δψ / V, distancia adicional lur_dist = Rr tan(0,5 Δψ).

Proyecciones de velocidad: V_e = V sin ψ (este), V_n = V cos ψ (norte). Tasas angulares: w_E = V_n/Rz, w_N = V_e/Rz.

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

El bucle de simulación para segmentos rectos y LUR asegura una trayectoria continua.

Filtro de Kalman discreto

Modelo dinámico: X' = F X + G W, Z = H X + V.

Forma discreta:

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

Donde Φ es la matriz de transición de estado, y Q/R representan las covarianzas de ruido del proceso y de medición.

Implementación en Engee con modelado de errores

El bucle de filtrado simula lecturas de sensores de giroscopios y acelerómetros con error cero, errores de factor de escala y ruido aleatorio:

• Giroscopios: zero_error = 0,001°/s, koef_error = 2×10⁻⁵, noise = 0,01×zero_error×randn()
• Acelerómetros: zero_error = g×60×10⁻⁶, koef_error = 3×10⁻⁶, noise similar

Fragmento de código:

for i=1:1:length(lam)-1
    i=i+1;
    # giroscopios
    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;
    # acelerómetros
    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

El filtro actualiza un vector de estado de 19 dimensiones X_out, almacenando trayectorias en X_out_list y matrices de covarianza en Pk_list.

Conclusiones clave

• Integración INS/GNSS: El filtro de Kalman elimina la acumulación de deriva en sistemas inerciales usando datos satelitales.
• Navegación por círculo máximo: Cálculos precisos de LUR con radio Rr = V²/(g tan γ).
• Modelado de errores: Deriva realista en giroscopios (0,001°/s) y sesgo en acelerómetros (60 μg).
• Implementación discreta: Paso de 0,1 segundos, matriz de transición de estado Φ derivada de dinámicas linealizadas.
• Scripts de Engee: Código completo para replicar el sistema de navegación integrado.

— Editorial Team