PSINS工具箱函数介绍——distance

关于工具箱

distance是计算轨迹长度并绘图的函数，位于文件夹$psinsbasebase2$里面
本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

• PSINS初学指导：https://blog.youkuaiyun.com/callmeup/article/details/137087932

使用方法

在PSINS工具箱里面的distance用于计算轨迹的长度。输入轨迹后，即可由这个函数迭代计算出来路程（也就是每个点之间的距离之和）。
此函数的输入输出形式为：

[dist, od] = distance(pos, timax)

• 输入pos为位置矢量，timax是迭代计算的最大时间间隔
• 输出dist为计算出来的距离值，od为一个2列的矩阵，第一列为每两个定位点之间的距离，第二列为时间戳
• 另外会有一个绘图，绘制路程随时间的变化曲线、高度变化曲线、二维轨迹图。
  【注】对于timax的理解为：如果pos数据的时间间隔为0.1，timax输入1，则会以0.1、0.2、0.4、0.8为时间间隔，分别计算四组轨迹长度。

例程实践

源代码

源代码如下：

function [dist, od] = distance(pos, timax)
    if nargin<2, timax=1; end
    if size(pos,2)>7, pos = pos(:,[7:9,end]); end
    if size(pos,2)==3, pos(:,4) = (1:size(pos,1))'; end
    tmin = diff(pos(1:2,end));
    myfigure;
    subplot(2,2,[2,4]), dxyz=pos2dxyz(pos); plot(dxyz(:,1), dxyz(:,2)); xygo('E / m', 'N / m');
    hold on, plot(0,0,'or');
    subplot(223), plot(dxyz(:,end), dxyz(:,3)); xygo('hgt');
    subplot(221);
    for k=1:fix(log2(length(pos)))
        [RMh, clRNh] = RMRN(pos);
        dpos = [zeros(1,3);diff(pos(:,1:3))];
        dxyz = [dpos(:,2).*clRNh, dpos(:,1).*RMh, dpos(:,3)];
        if k==1, od = [normv(dxyz),pos(:,end)]; end
        distk = cumsum(normv(dxyz));
        plot(pos(:,end), distk); hold on;
        dist(k,1) = distk(end);
        pos = pos(1:2:end,:);
        tmax = diff(pos(1:2,end));
        if tmax>timax, break; end
    end
    xygo('distance / m');
    title(sprintf('Distance max=%.3fm(%.3fs), min=%.3fm(%.3fs)',dist(1),tmin,dist(end),tmax));

运行代码，下面分析代码运行的结果：

运行结果

以工具箱的“test_SINS.m”这个函数下的轨迹为例，先运行这个test函数，再运行：

distance(trj.avp(:,7:10));

函数输入量trj.avp是轨迹的avp，共10列：

• 第1~3列是角度
• 第4~6列是速度
• 第7~9列是位置
• 第10列是时间戳
  所以trj.avp(:,7:10)是轨迹的三轴位置数据和时间戳数据。

运行函数后，得到的轨迹绘图和计算的路程值：

左上角的图是路程与时间的关系，表头显示的数字$7586$即为路程。左下角是高度与时间的关系。
右边的图为二维轨迹图，与insplot相仿（参考链接：PSINS工具箱函数介绍——insplot）

函数解析

函数输入为位置（纬度+精度+高度）和时间戳。输出为计算出的来的路程。
如果没有时间戳，函数内部会自动添加，默认时间间隔为1s。

尤其要注意输入的pos带时间戳的情况

一旦自带的时间戳间隔不为1，则很大可能会生成多个路径长度，并在绘图窗口中展示max和min，如下蓝框：

具体来看，是这样的：

1. 如果pos的长度为64，则$lo{g}_2(100)=6$，则最多可以以一倍的采样率、2倍、4倍、8倍、16倍、32倍¹共最多产生6个不同采样周期的输出
2. 在对采样周期进行翻倍时，程序还会对新的采样周期进行检测，一旦大于timax则停止计算→并输出

---

1. 64倍时，第64个点作为第一个点，全过程只有一个位置，单点无法计算距离） ↩︎