Kalman Filter for INS and GNSS in Engee

Optimal Kalman Filter for INS/GNSS Integration in Engee

Unmanned aerial vehicles (UAVs) rely on a combination of inertial navigation systems (INS) and satellite navigation systems (GNSS) to accurately determine motion parameters. The optimal Kalman filter corrects accumulated INS errors using GNSS data, providing precise position and velocity updates. The implementation is carried out in the Engee simulation environment with a discrete model step of 0.1 seconds.

INS operates under perturbed conditions due to gyroscope and accelerometer inaccuracies, initial condition errors, and computational drift. The core equation of inertial navigation is R' = V_n + g(R) + n, where R is the position vector, V_n is apparent velocity, g is the gravity gradient.

GNSS delivers positioning accuracy of 10–12 meters and velocity accuracy of 0.05 m/s, improving to 1–2 meters in differential mode. Errors stem from ephemeris inaccuracies, ionospheric and tropospheric delays, and receiver noise.

Initialization and Simulation Parameters

The flight is simulated at an altitude of 1,000 meters along a route with turning points: latitudes [55°, 56.58°, 56.46°, 56.96°], longitudes [37°, 37.5°, 38°, 38.5°]. Constants used: Rz = 6,371,000 m, g = 9.78049 m/s², dt = 0.1 s, t = 3,600 s.

Initial errors:

• λ_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° (heading)
• γ_error = 0.03° (roll)
• θ_error = 0.03° (pitch)

Initialization code for 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);
# ... (other arrays)

Turning points:

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

Trajectory Calculation via Great Circle Navigation

Great circle navigation defines the shortest path on a sphere. For each segment of the route, the following are computed:

• Longitude change: Δλ = λ_{j} - λ_{j-1}
• Great circle distance: σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• Distance: D = Rz ⋅ σ
• Heading: ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

Heading ψ is normalized to the range [0, 2π].

Total distance sum(D), average speed V = sum(D)/t.

Code for heading calculation:

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

Trajectory Construction with Left/Right Turns (LUR)

LUR (left/right turn): radius Rr = V²/(g tan γ), time Tr = Rr ⋅ Δψ / V, additional distance lur_dist = Rr tan(0.5 Δψ).

Velocity projections: V_e = V sin ψ (east), V_n = V cos ψ (north). Angular rates: w_E = V_n/Rz, w_N = V_e/Rz.

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

The simulation loop for straight segments and LUR ensures a continuous trajectory.

Discrete Kalman Filter

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

Discrete 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

Where Φ is the state transition matrix, and Q/R represent process and measurement noise covariances.

Implementation in Engee with Error Modeling

The filtering loop simulates sensor readings from gyroscopes and accelerometers with zero bias, scale factor errors, and random noise:

• Gyroscopes: zero_error = 0.001°/s, koef_error = 2×10⁻⁵, noise = 0.01×zero_error×randn()
• Accelerometers: zero_error = g×60×10⁻⁶, koef_error = 3×10⁻⁶, noise similar

Fragment 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;
    # accelerometers
    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

The filter updates a 19-dimensional state vector X_out, storing trajectories in X_out_list and covariance matrices in Pk_list.

Key Takeaways

• INS/GNSS Integration: The Kalman filter eliminates drift accumulation in inertial systems using satellite data.
• Great Circle Pathing: Accurate LUR calculations with radius Rr = V²/(g tan γ).
• Error Modeling: Realistic gyroscope drift (0.001°/s) and accelerometer bias (60 μg).
• Discrete Implementation: 0.1-second step, state transition matrix Φ derived from linearized dynamics.
• Engee Scripts: Complete codebase for replicating the integrated navigation system.

— Editorial Team