PSINS工具箱函数介绍——distance

关于工具箱

kfinit是kf的参数初始化函数，用于初始化滤波参数
本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

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

使用方法

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

例程实践

源代码

源代码如下：

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