The simulation of 3D state combination navigation uses the Cubature Kalman Filter (CKF) for nonlinear filtering. The state model consists of 15-dimensional error states, and the observation model includes 6-dimensional GNSS observations (position + velocity).

Article Directory

• Program Introduction
• Running Results
• MATLAB Source Code

Program Introduction

This program implements a 3D state combination navigation simulation, utilizing the Cubature Kalman Filter (CKF) for nonlinear filtering. The state model is a 15-dimensional error state that includes:

The observation model consists of 6-dimensional GNSS observations (position + velocity). The trajectory is set to a spiral ascent motion to test the performance of the filter.

This example is based on a rigorous derivation of combination navigation, using CKF as the filtering method, which can achieve high-precision estimation of states such as position and velocity. The program framework is clear, and the comments are detailed, making it easy to understand the practical application and effects of CKF in combination navigation.

Features:

• Utilizes an 8-dimensional error state model covering core factors such as position, velocity, heading, and sensor biases;
• Provides 2D position observation input, closely aligned with practical application scenarios;
• Structured code design, facilitating secondary development and expansion;
• Suitable as a reference case for research learning, course design, and engineering applications.

Running Results

3D trajectory comparison:Position comparison curves for each axis:Velocity comparison curves for each axis:Attitude comparison curves for each axis:Error comparison curves:

Error statistical characteristics from the command line output:

MATLAB Source Code

The program structure is as follows:

Part of the code is as follows:

% 3D state CKF example (rigorous combination navigation derivation),156
% Based on a 15-dimensional error state model: position(3), velocity(3), attitude(3), gyro bias(3), accelerometer bias(3)
% Based on 6-dimensional observations: XYZ position (3) + XYZ velocity (3)
% Cubature Kalman Filter implementation
% Author: matlabfilter (WeChat same number), custom MATLAB code related to positioning and navigation, filtering
% 2025-08-28/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.01*pi/180;% Gyro noise standard deviation (rad/s)
accel_noise_std =0.001;% Accelerometer noise standard deviation (m/s^2)
gyro_bias_std =0.001*pi/180;% Gyro bias standard deviation (rad/s)
accel_bias_std =0.0001;% Accelerometer bias standard deviation (m/s^2)

% GNSS observation noise
gnss_pos_noise_std =0.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 (15×15)
% State order: [position(3), velocity(3), attitude(3), gyro bias(3), accelerometer bias(3)]
Q =zeros(15,15);
% Position noise (generated by integrating velocity)
Q(1:3,1:3)=eye(3)*(accel_noise_std * dt^2/2)^2;
% Velocity noise
Q(4:6,4:6)=eye(3)*(accel_noise_std * dt)^2;
% Attitude noise
Q(7:9,7:9)=eye(3)*(gyro_noise_std * dt)^2;
% Gyro bias noise
Q(10:12,10:12)=eye(3)*(gyro_bias_std * dt)^2;
% Accelerometer bias noise
Q(13:15,13:15)=eye(3)*(accel_bias_std * dt)^2;

%% Observation noise covariance matrix R (6×6)
% Observations: GNSS position(3) + GNSS velocity(3)
R =zeros(6,6);
R(1:3,1:3)=eye(3)* gnss_pos_noise_std^2;
R(4:6,4:6)=eye(3)* gnss_vel_noise_std^2;

%% Initialization

Complete code:

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

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

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