clear ; clc; load('IMUdata01.mat'); dVe=IMUdata01(:,15); %-东向速度 dVn=IMUdata01(:,16); %-北向速度 % dVu=IMUdata01(:,17); %-天向速度 dVv=IMUdata01(:,18); %-总的速度 dheading=IMUdata01(:,3)*pi/180; %偏航角heading NN0=3001; %起始点 NN=200; %点的个数 DVv=dVv(NN0:NN0+NN-1,1); %从起始点NN0开始的NN个点的车辆速度 DVe=dVe(NN0:NN0+NN-1,1); %从起始点NN0开始的NN个点的东向速度 DVn=dVn(NN0:NN0+NN-1,1); %从起始点NN0开始的NN个点的北向速度 Dheading=dheading(NN0:NN0+NN-1,1); %从起始点NN0开始的NN个点的偏航角 % DVu=dVu(NN0:NN0+NN-1,1); Beta=pi/2-Dheading; %车辆前进方向与惯性坐标系下的x轴的夹角 %%1.以东向速度与北向速度积分得到车辆在惯性坐标系下的轨迹 XE=zeros(NN,1); %积分得出的东向坐标 YN=zeros(NN,1); %积分得到的北向坐标 % ZU=zeros(NN,1); %积分得到的天向坐标 for i=2:NN T = 0.1; %中值积分得到三轴位置（相对大地坐标系的） XE(i)=XE(i-1)+0.5*(DVe(i-1)+DVe(i))*T; YN(i)=YN(i-1)+0.5*(DVn(i-1)+DVn(i))*T; % ZU(i)=ZU(i-1)+0.5*(DVu(i-1)+DVu(i))*T; end %%2.选取点 Sc=1; %定一个选择点的标准，这里是1米 falg00=1; ii=1; s0=0; %速度积分得到的距离 T=0.1; LG=32; %需要存的数据点数 Guijidata=zeros(3,LG); %输出的三个量，XE,YN，Beta； jj=1; while falg00 s0=s0+(DVv(ii+1)+DVv(ii))*T/2; if (s0>Sc)||(s0==Sc) for jj1=1:LG-1 Guijidata(1,jj1)=Guijidata(1,jj1+1); Guijidata(2,jj1)=Guijidata(2,jj1+1); Guijidata(3,jj1)=Guijidata(3,jj1+1); end Guijidata(1,LG)=XE(ii+1); Guijidata(2,LG)=YN(ii+1); Guijidata(3,LG)=Beta(ii+1); s0=0; end ii=ii+1; if ii>NN-1 falg00=0; end end 把%%1后面写成子函数

可以将%%1后面的代码封装成一个名为"integrate_position"的子函数，代码如下：

function [XE, YN, Beta] = integrate_position(DVe, DVn, Dheading)
% INTEGRATE_POSITION integrates the eastward velocity, northward velocity
% and heading to obtain the vehicle position in the inertial coordinate
% system.
%
% INPUTS:
% DVe - Eastward velocity.
% DVn - Northward velocity.
% Dheading - Heading angle.
%
% OUTPUTS:
% XE - Eastward position.
% YN - Northward position.
% Beta - Vehicle forward direction with respect to the x-axis of the
%        inertial coordinate system.

NN = length(DVe);
XE = zeros(NN,1);
YN = zeros(NN,1);
Beta = pi/2 - Dheading;

for i = 2:NN
    T = 0.1;
    XE(i) = XE(i-1) + 0.5*(DVe(i-1) + DVe(i))*T;
    YN(i) = YN(i-1) + 0.5*(DVn(i-1) + DVn(i))*T;
end

end

使用时只需要调用函数即可，例如：

[XE, YN, Beta] = integrate_position(DVe, DVn, Dheading);