Engee에서 INS와 GNSS를 위한 Kalman Filter

Julia에서 INS/GNSS 통합을 위한 최적 칼만 필터

무인 항공기(UAV)는 운동 매개변수를 정확히 판단하기 위해 관성 항법 시스템(INS)과 위성 항법 시스템(GNSS)의 조합에 의존합니다. 최적의 칼만 필터는 GNSS 데이터를 활용해 누적된 INS 오차를 보정하여 정밀한 위치와 속도 업데이트를 제공합니다. 이 구현은 0.1초 단위의 이산 모델로 Julia 시뮬레이션 환경에서 수행됩니다.

INS는 자이로스코프와 가속도계의 불완전성, 초기 조건 오류, 계산 드리프트로 인해 외란 상태에서 작동합니다. 관성 항법의 핵심 방정식은 R' = V_n + g(R) + n이며, 여기서 R는 위치 벡터, V_n는 상대 속도, g는 중력 기울기입니다.

GNSS는 위치 정확도 10~12미터, 속도 정확도 0.05m/s를 제공하며, 차등 모드에서는 1~2미터 수준으로 향상됩니다. 오차 원인은 에포메리스 부정확성, 이온권 및 대류권 지연, 수신기 노이즈 등입니다.

초기화 및 시뮬레이션 파라미터

비행은 고도 1,000미터에서 전환점이 있는 경로를 따라 시뮬레이션됩니다: 위도 [55°, 56.58°, 56.46°, 56.96°], 경도 [37°, 37.5°, 38°, 38.5°]. 사용된 상수: Rz = 6,371,000 m, g = 9.78049 m/s², dt = 0.1 s, t = 3,600 s.

초기 오차:

• λ_error = 6/Rz (경도)
• φ_error = 6/Rz (위도)
• V_e_error = 0.05 m/s
• V_n_error = 0.05 m/s
• ψ_error = 0.25° (방향)
• γ_error = 0.03° (롤)
• θ_error = 0.03° (피치)

배열 초기화 코드:

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);
# ... (다른 배열)

전환점:

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

대원항법을 통한 궤적 계산

대원항법은 구면상에서 가장 짧은 경로를 정의합니다. 각 경로 세그먼트에 대해 다음을 계산합니다:

• 경도 변화: Δλ = λ_{j} - λ_{j-1}
• 대원 거리: σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• 거리: D = Rz ⋅ σ
• 방향각: ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

방향각 ψ는 [0, 2π] 범위로 정규화됩니다.

총 거리 sum(D), 평균 속도 V = sum(D)/t.

방향각 계산 코드:

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

좌우 회전(LUR)을 포함한 궤적 구성

LUR(좌우 회전): 반경 Rr = V²/(g tan γ), 시간 Tr = Rr ⋅ Δψ / V, 추가 거리 lur_dist = Rr tan(0.5 Δψ).

속도 투영: V_e = V sin ψ (동쪽), V_n = V cos ψ (북쪽). 각속도: w_E = V_n/Rz, w_N = V_e/Rz.

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

직선 세그먼트와 LUR에 대한 시뮬레이션 루프는 연속적인 궤적을 보장합니다.

이산 칼만 필터

동적 모델: X' = F X + G W, Z = H X + V.

이산 형태:

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

여기서 Φ는 상태 전이 행렬이며, Q/R는 과정 및 측정 노이즈 공분산을 나타냅니다.

INS에서 오류 모델링을 통한 필터 구현

필터 루프는 자이로스코프와 가속도계의 센서 읽기를 제로 바이어스, 스케일 팩터 오류, 무작위 노이즈를 포함해 시뮬레이션합니다:

• 자이로스코프: zero_error = 0.001°/s, koef_error = 2×10⁻⁵, noise = 0.01×zero_error×randn()
• 가속도계: zero_error = g×60×10⁻⁶, koef_error = 3×10⁻⁶, noise 유사

코드 조각:

for i=1:1:length(lam)-1
    i=i+1;
    # 자이로스코프
    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;
    # 가속도계
    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

필터는 19차원 상태 벡터 X_out을 업데이트하고, 궤적은 X_out_list에 저장되며 공분산 행렬은 Pk_list에 저장됩니다.

핵심 요약

• INS/GNSS 통합: 칼만 필터는 위성 데이터를 활용해 관성 시스템의 드리프트 누적을 제거합니다.
• 대원 경로 계획: 반경 Rr = V²/(g tan γ)를 기반으로 정확한 LUR 계산이 가능합니다.
• 오류 모델링: 현실적인 자이로스코프 드리프트(0.001°/s)와 가속도계 바이어스(60 μg)를 반영합니다.
• 이산 구현: 0.1초 간격, 선형화된 동역학에서 도출된 상태 전이 행렬 Φ를 사용합니다.
• Julia 스크립트: 통합 항법 시스템을 재현하기 위한 완전한 코드베이스가 제공됩니다.

— Editorial Team