代码用于模拟和校正三维惯性导航系统（INS）的轨迹漂移。通过线性分配终点误差，实现对累积漂移的补偿。为INS漂移提供一个非滤波的思路。

背景

算法原理

本代码针对惯性导航系统（INS）累积误差导致的三维轨迹漂移问题，提出基于终点误差线性分配的校正方法。其核心思想是通过起点与终点的真实位置偏差，动态生成全局补偿矩阵，实现系统性误差的时空关联修正。

漂移误差建模

• 速度层噪声：
  $$\Delta v(t)=\sum _{k=1}^n{\eta }_v(k)\cdot \sqrt{\Delta t},{\eta }_v\sim \mathcal{N}(0,{\sigma }_v^2)$$
  式中${\sigma }_v=0.005$为速度噪声强度（代码第23行），模拟随机游走过程。

• 位移层噪声：
  $$\Delta p(t)={\int }_0^t\Delta v(\tau )d\tau =\sum _{k=1}^n\Delta v(k)\cdot \Delta t$$
  对应代码中的cumsum双重积分操作（第24行），符合布朗运动模型。

终点校正策略

• 闭合差计算：
  $$\delta ={\mathbf{p}}_{\text{true\_end}}-{\mathbf{p}}_{\text{drift\_end}}=[\Delta x,\Delta y,\Delta z{]}^T$$
  计算真实终点与漂移终点的矢量偏差（代码第29行）。

• 线性分配校正：
  $${\mathbf{C}}_{\text{correction}}=\delta \otimes \mathbf{w},w_i=\frac{i-1}{n-1}$$
  通过时间权重向量(\mathbf{w})将全局误差分配到各轨迹点，校正后轨迹为：
  $${\mathbf{P}}_{\text{corrected}}={\mathbf{P}}_{\text{drift}}+{\mathbf{C}}_{\text{correction}}$$
  该操作时间复杂度为$O(n)$，适用于实时处理

关键代码模块解析

1. 轨迹仿真模块

   • 生成理想轨迹：true_position = cumsum(true_velocity .* t)
   • 添加双重积分噪声：ins_position = true_position + noise_drift_pos

2. 校正核心模块

   • 计算校正矩阵：correction_matrix = delta * correction_factors
   • 应用校正：corrected_position = ins_position + correction_matrix

3. 可视化模块

   • 三维轨迹对比：plot3绘制漂移/校正/真实轨迹
   • 误差曲线：分轴显示校正前后误差绝对值分布

扩展方向

1. 动态权重优化：替换线性分配为指数函数$w_i=\frac{1}{1+e^{-\lambda t}}$，适应非均匀漂移
2. 多传感器融合：结合GNSS观测值构建紧耦合校正模型
3. 非线性补偿：引入B样条曲线拟合真实轨迹，抑制高频噪声

运行结果

三维轨迹对比：

距离误差曲线：

各轴误差对比：

MATLAB代码

程序结构：

部分代码：

% 轨迹漂移的终点校正例程，适用于三维的情况
% 2025-06-24/Ver1
clear; clc; close all;
rng(0); % 固定随机数种子，便于复现

% 轨迹点数量
n = 200; % 轨迹点数量
t = linspace(0, 10, n); % 时间序列

% 模拟 INS 解算的轨迹 (包含漂移)
true_velocity = [2; 1; 0.5]; % 真实速度向量 (m/s)

%% INS 模拟：积分得到位置 + 漂移噪声
true_position = cumsum(true_velocity .* t); % 理想轨迹（无漂移）
noise_drift_vel = cumsum(0.005 * randn(3, n), 2); % 速度层面的累积漂移（随机游走）
noise_drift_pos = cumsum(noise_drift_vel,2); %位移层面的累积漂移
ins_position = true_position + noise_drift_pos; % 加入漂移后的轨迹

%% 已知的起点和真实终点
start_point = ins_position(:, 1); % 起点
true_end_point = true_position(:, end); % 真实终点
drift_end_point = ins_position(:, end); % 漂移后的终点

% 漂移矢量计算
delta = true_end_point - drift_end_point;

%% 校正轨迹
% 误差统计
% 校正前误差


%% 校正后误差计算



% 打印误差统计结果


%% 可视化轨迹


% 绘制误差分布

完整代码与讲解：
https://blog.csdn.net/callmeup/article/details/148877350?sharetype=blogdetail&sharerId=148877350&sharerefer=PC&sharesource=callmeup&spm=1011.2480.3001.8118

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者