• Using an 8-dimensional state model, it comprehensively describes position, velocity, attitude, and sensor biases in planar motion. The Cubature Kalman Filter is employed to handle nonlinear problems, offering better numerical stability and accuracy compared to traditional EKF, effectively fusing IMU and GNSS data to fully leverage the complementary advantages of both sensors.

Table of Contents

• Program Overview
• Simulation Scenario
• Noise Model
• Results
• MATLAB Source Code

Program Overview

Core Innovations:

• Utilizes an 8-dimensional state model to comprehensively describe position, velocity, attitude, and sensor biases in planar motion.
• Employs the Cubature Kalman Filter to address nonlinear issues, providing better numerical stability and accuracy compared to traditional EKF.
• Achieves effective fusion of IMU and GNSS, fully utilizing the complementary advantages of both sensors.State Model

8-Dimensional Error State Model:

Position: 2D (X, Y coordinates)Velocity: 2D (X, Y directional velocity)Heading Angle: 1D (Yaw angle)Gyroscope Bias: 1D (Angular velocity bias)Accelerometer Bias: 2D (X, Y directional acceleration bias)

Observation Model

2D Observation: XY position information provided by GNSSObservation Frequency: GNSS updates every 1 second (10Hz IMU, 1Hz GNSS)

Core Algorithm

Cubature Kalman Filter: Utilizes spherical-radial integration rules to approximate state distribution using 2n volume points.Nonlinear System Handling: Effectively addresses nonlinear issues in attitude updates.

Simulation Scenario

Trajectory Type: Circular MotionMotion Parameters:

Radius: 50 metersAngular Velocity: 0.1 rad/sSimulation Time: 100 secondsSampling Frequency: 10Hz

Noise Model

IMU Noise:

Gyroscope Noise: 0.1°/sAccelerometer Noise: 0.01 m/s²Gyroscope Bias: 0.01°/sAccelerometer Bias: 0.001 m/s²

GNSS Noise:

Position Measurement Noise: 3 meters standard deviation

Results

Trajectory Comparison:Comparison of positions and velocities on both axes:Error comparison curve:

Screenshot of the command line window:

MATLAB Source Code

Partial code is as follows:

% Planar CKF Example (Strict Integrated Navigation Derivation),82
% Based on 8-dimensional error state model: Position(2), Velocity(2), Heading Angle, Heading Bias(1), Accelerometer Bias(2)
% Based on 4-dimensional observation: XY position (2)
% Cubature Kalman Filter implementation
% Author: matlabfilter (WeChat same number), for custom MATLAB code related to positioning and navigation, filtering
% 2025-08-30/Ver1

clear; clc; close all;
rng(0);% Fix 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;% Gyro noise standard deviation (rad/s)
accel_noise_std =0.01;% Accelerometer noise standard deviation (m/s^2)
gyro_bias_std =0.01*pi/180;% Gyro 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 (8×8)
% State order: [Position(2), Velocity(2), Attitude(1), Gyro Bias(1), Accelerometer Bias(2)]
Q =zeros(8,8);
% Position noise (generated by velocity integration)
Q(1:2,1:2)=eye(2)*(accel_noise_std * dt^2)^2;
% Velocity noise
Q(3:4,3:4)=eye(2)*(accel_noise_std * dt)^2;
% Attitude noise
Q(5,5)=eye(1)*(gyro_noise_std * dt)^2;
% Gyro bias noise
Q(6,6)=eye(1)*(gyro_bias_std * dt)^2;
% Accelerometer bias noise
Q(7:8,7:8)=eye(2)*(accel_bias_std * dt)^2;

%% Observation Noise Covariance Matrix R (4×4)
% Observations: GNSS position(2)
R =eye(2)* gnss_pos_noise_std^2;

Full code:

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

Or click “Read the original text” at the end to jump.

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