代码用于模拟和校正三维惯性导航系统（INS）的轨迹漂移。通过线性分配终点误差，实现对累积漂移的补偿。为INS漂移提供一个非滤波的思路。
文章目录
- 背景
- 算法原理
- 关键代码模块解析
- 扩展方向
- 运行结果
- MATLAB代码
背景
算法原理
本代码针对惯性导航系统（INS）累积误差导致的三维轨迹漂移问题，提出基于终点误差线性分配的校正方法。其核心思想是通过起点与终点的真实位置偏差，动态生成全局补偿矩阵，实现系统性误差的时空关联修正。
漂移误差建模
速度层噪声：
Δ v ( t ) = ∑ k = 1 n η v ( k ) ⋅ Δ t , η v ∼ N ( 0 , σ v 2 ) \Delta v(t) = \sum_{k=1}^n \eta_v(k) \cdot \sqrt{\Delta t}, \quad \eta_v \sim \mathcal{N}(0, \sigma_v^2)Δv(t)=k=1∑nηv(k)⋅Δt,ηv∼N(0,σv2)
式中σ v = 0.005 \sigma_v=0.005σv=0.005为速度噪声强度（代码第23行），模拟随机游走过程。位移层噪声：
Δ p ( t ) = ∫ 0 t Δ v ( τ ) d τ = ∑ k = 1 n Δ v ( k ) ⋅ Δ t \Delta p(t) = \int_0^t \Delta v(\tau) d\tau = \sum_{k=1}^n \Delta v(k) \cdot \Delta tΔp(t)=∫0tΔv(τ)dτ=k=1∑nΔv(k)⋅Δt
对应代码中的cumsum双重积分操作（第24行），符合布朗运动模型。
终点校正策略
闭合差计算：
δ = p true_end − p drift_end = [ Δ x , Δ y , Δ z ] T \delta = \mathbf{p}_{\text{true\_end}} - \mathbf{p}_{\text{drift\_end}} = [\Delta x, \Delta y, \Delta z]^Tδ=ptrue_end−pdrift_end=[Δx,Δy,Δz]T
计算真实终点与漂移终点的矢量偏差（代码第29行）。线性分配校正：
C correction = δ ⊗ w , w i = i − 1 n − 1 \mathbf{C}_{\text{correction}} = \delta \otimes \mathbf{w}, \quad w_i = \frac{i-1}{n-1}Ccorrection=δ⊗w,wi=n−1i−1
通过时间权重向量(\mathbf{w})将全局误差分配到各轨迹点，校正后轨迹为：
P corrected = P drift + C correction \mathbf{P}_{\text{corrected}} = \mathbf{P}_{\text{drift}} + \mathbf{C}_{\text{correction}}Pcorrected=Pdrift+Ccorrection
该操作时间复杂度为O ( n ) O(n)O(n)，适用于实时处理
关键代码模块解析
轨迹仿真模块
- 生成理想轨迹：
true_position = cumsum(true_velocity .* t) - 添加双重积分噪声：
ins_position = true_position + noise_drift_pos
- 生成理想轨迹：
校正核心模块
- 计算校正矩阵：
correction_matrix = delta * correction_factors - 应用校正：
corrected_position = ins_position + correction_matrix
- 计算校正矩阵：
可视化模块
- 三维轨迹对比：
plot3绘制漂移/校正/真实轨迹 - 误差曲线：分轴显示校正前后误差绝对值分布
- 三维轨迹对比：
扩展方向
- 动态权重优化：替换线性分配为指数函数w i = 1 1 + e − λ t w_i = \frac{1}{1 + e^{-\lambda t}}wi=1+e−λt1，适应非均匀漂移
- 多传感器融合：结合GNSS观测值构建紧耦合校正模型
- 非线性补偿：引入B样条曲线拟合真实轨迹，抑制高频噪声
运行结果
三维轨迹对比：
距离误差曲线：
各轴误差对比：
MATLAB代码
程序结构：
部分代码：
% 轨迹漂移的终点校正例程，适用于三维的情况% 2025-06-24/Ver1clear;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
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
