PSINS工具箱函数介绍—

PSINS工具箱函数介绍——gpssimu

news2024/11/14 15:12:04

在这里插入图片描述

关于工具箱

gpssimu是生成GPS的位置和速度信息的函数，在psins240101\base\base1目录下
本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

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

gpssimu是生成带误差的GPS位置和速度的函数

函数作用：根据预定的轨迹（avp信息）和相关的误差、延迟等信息，计算出来相对于SIMU的带误差的GPS数据。误差考虑的因素较多，比较准确。
输入与输出：

输入：AVP信息、速度误差（标准差）、位置误差（标准差）、一节马尔可夫相关的时间带来的位置误差、GPS与SIMU之间的摇臂误差、SIMU与GPS之间的
输出：带误差的GPS数据，包括：三轴速度、三轴位置（纬经高）、时间戳

例程实践

使用工具箱的“trj10ms.mat”中的数据来进行实验，运行以下程序：

glvs;
load('trj10ms.mat');
out = gpssimu(trj.avp,[1;1;1],[1;1;1]);

上面的 $o u t = g p ss im u (t r j . a v p, [1; 1; 1], [1; 1; 1]);$ 表示：在三轴速度上加上1m/s的误差，在三轴位置上加上1°的误差。

运行结果

得到的out如下：
在这里插入图片描述
对out的前三项（东北天三轴速度）绘图，得到的图像如下：

对比速度的真实值（如下图）：

可以看出来误差添加成功

源代码

function gpsVnPos = gpssimu(avp, dvn, dpos, tau, lever, imu_delay, isplot)

global glv
    if nargin<7,  isplot=0;   end
    if nargin<6,  imu_delay=0;   end
    if nargin<5,  lever=0;   end
    if nargin<4,  tau=1;   end
    if length(lever)==1,  lever=[lever;lever;lever];   end
    if length(tau)==1,   tau=[tau;tau];   end
    if length(tau)==2,   tau=[tau(1);tau(1);tau(1);tau(2);tau(2);tau(2)];   end
    if length(dvn)==1;   dvn=[dvn;dvn;dvn];   end
    if length(dpos)==1;   dpos=poserrset(dpos);   end
    %% find the nearest second in avp time
    if length(avp)==7  % avp = [vn0; pos0; T]; % for static
        T = fix(avp(end));
        avp = [zeros(T,3), repmat(avp(1:6)', T,1), (1:T)'];
    end
    idx = zeros(size(avp(:,end))); rt = idx; kk=1;
    gt = imu_delay;
    for k=1:length(avp)-1
        while gt<avp(k,end)
            gt = gt + 1;
        end
        if avp(k,end)<=gt && gt<avp(k+1,end)
            idx(kk) = k; rt(kk) = gt-avp(k,end); kk=kk+1;
        end
    end
    idx(kk:end) = [];
    %% add error
    ts = avp(2,end)-avp(1,end);
    gpsVnPos = zeros(length(idx),7); kk=1;
    for k=1:length(idx) % lever arm + time delay
        ik = idx(k);
        avpi = avpinterp(avp(ik,1:9),avp(ik+1,1:9),rt(k)/ts)';
%         wnb = a2cwa(avp(ik+1,1:3)')*(avp(ik+1,1:3)-avp(ik,1:3))'/ts; % wnb~=web
        wnb = m2rv(a2mat(avp(ik,1:3)')'*a2mat(avp(ik+1,1:3)'))/ts;
        vngps = avpi(4:6)+a2mat(avpi(1:3)')*cros(wnb,lever);
        gpsVnPos(kk,:) = [vngps; avpi(7:9)+la2dpos(avpi, lever); round(avp(ik,end))]';  kk = kk+1;
    end
    dp = markov1([dvn;dpos], tau, 1, length(idx));  % 1st-order Markov error
    gpsVnPos(:,1:end-1) = gpsVnPos(:,1:end-1) + dp;
    if(isplot==1)
        gpsVnPos0 = gpsVnPos(1,1:6); gpsVnPos(1,1:6) = avp(1,4:9);
        gpsplot(gpsVnPos);
        subplot(221), hold on, plot(avp(:,end), avp(:,4:6), 'm-.');
        subplot(223), hold on, 
        plot(avp(:,end), [[avp(:,7)-avp(1,7),(avp(:,8)-avp(1,8))*cos(avp(1,7))]*glv.Re,avp(:,9)-avp(1,9)], 'm-.');
        gpsVnPos(1,1:6) = gpsVnPos0;
    end