The program fully demonstrates the modeling and implementation of UKF in 2D plane for IMU+GNSS integrated navigation, clearly illustrating the filtering effect against sensor noise and drift.

Table of Contents

• Program Overview
• Program Introduction
• Core Steps
• Program Functionality
• Results
• MATLAB Source Code

Program Overview

Program Introduction

This program implements a simulation of Unscented Kalman Filter (UKF) for integrated navigation based on the error state model, with state variables including position, velocity, heading, gyroscope bias, and accelerometer bias. The program mainly compares the pure IMU integration results with the UKF estimation results in terms of position and velocity error performance, and calculates the root mean square error (RMSE).

The simulation trajectory adopts a 2D circular motion, achieving data fusion through IMU (accelerometer + gyroscope) and GNSS (position + velocity observations).

Core Steps

1. State Modeling

2. Process Model (Prediction)

3. UKF Prediction and Update

Program Functionality

• Generate true trajectory, IMU and GNSS observation data;
• Implement pure IMU integration navigation as a comparison benchmark;
• Implement UKF integrated navigation, significantly suppressing error drift;
• Plot trajectory comparison, position error, and velocity error curves;
• Calculate RMSE to quantify filtering effects.

Results

Illustration of the 2D trajectories obtained by various methods:Comparison of XY axis position curves:Comparison of XY axis velocity curves:Comparison of position error curves:

It can be seen that the error of the UKF estimation is significantly smaller than that of the pure IMU (before filtering).

MATLAB Source Code

The complete code is as follows:

<span>% UKF example for 2D state variables (with strict derivation of integrated navigation)</span>
<span>% Based on 8-dimensional error state model: position, velocity, heading, gyroscope bias, accelerometer bias</span>
<span>% Author contact: WeChat matlabfilter (for paid consultation unless prior agreement)</span>
<span>% 2025-08-24/Ver1</span>
clear
clc
close all
rng(0); % Fixed random seed
%% System parameter settings
dt = 0.1; % Sampling time interval (s)
total_time = 100; % Total simulation time (s)
N = total_time / dt; % Number of sampling points
%% Noise parameter settings
% IMU noise parameters
gyro_noise_std = 0.1 * pi / 180; % Gyroscope noise standard deviation (rad/s)
accel_noise_std = 0.01; % Accelerometer noise standard deviation (m/s^2)
gyro_bias_std = 0.01 * pi / 180; % Gyroscope bias standard deviation (rad/s)
accel_bias_std = 0.001; % Accelerometer bias standard deviation (m/s^2)
% GNSS observation noise
gnss_pos_noise_std = 3; % GNSS position noise standard deviation (m)
gnss_vel_noise_std = 0.1; % GNSS velocity noise standard deviation (m/s)
%% Process noise covariance matrix Q
...

Complete code download link:

https://mall.bilibili.com/neul-next/detailuniversal/detail.html?isMerchant=1&page=detailuniversal_detail&saleType=10&itemsId=12980185&loadingShow=1&noTitleBar=1&msource=merchant_share

Or click on the “Read the original text” at the end.

<span>If you need help or have custom code requirements related to navigation and positioning filtering, please contact the author via WeChat below</span>