PSINS工具箱函数介绍——trjsimu

关于工具箱

trjsimu是生成轨迹的的函数，用于初始化IMU数据和avp
本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

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

使用方法

运行下列代码后：

glvs
ts = 0.1;
avp0 = [[0;0;0]; [0;0;0]; glv.pos0];
xxx = [];
seg = trjsegment(xxx, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   20, xxx, 5);
seg = trjsegment(seg, 'uniform',      10);
seg = trjsegment(seg, 'climb',        15, 2, xxx, 20);
seg = trjsegment(seg, 'uniform',      50);
seg = trjsegment(seg, '8turn',        360, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      50);
seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4);
for k=1:2
seg = trjsegment(seg, 'uniform',      380);
seg = trjsegment(seg, 'coturnright',  90, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      380);
seg = trjsegment(seg, 'coturnleft',   90, 2, xxx, 4);
end
seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      550);
seg = trjsegment(seg, 'coturnleft',   105, 180/105, xxx, 4);
seg = trjsegment(seg, 'uniform',      50);
seg = trjsegment(seg, 'descent',      15, 2, xxx, 20);
seg = trjsegment(seg, 'uniform',      30);
seg = trjsegment(seg, 'deaccelerate', 20, xxx, 5);
seg = trjsegment(seg, 'uniform',      3600-sum(seg.wat(:,1)));

再运行关于trjsimu的代码：

trj = trjsimu(avp0, seg.wat, ts, 1);

即可得到trj这个结构体。

运行结果

trj这个结构体的内容如下：

包含imu数据、avp数据、avp初值等。

函数作用

可见，trj的作用是根据前面的路径$seg$来计算avp和imu数据的。

函数源码

把源代码调出来，可以看到如下内容：

function trj = trjsimu(avp0, wat, ts, repeats)
global glv
    if nargin<4, repeats = 1; end
    wat1 = repmat(wat, repeats, 1);
    damping = 1-exp(-ts/5.0); % damping=0;
    att = avp0(1:3); vn = avp0(4:6); pos = avp0(7:9);
    eth = earth(pos, vn);  Cbn_1 = a2mat(att)';  wm_1 = [0;0;0]; ts2 = ts/2;
%     Mpv = [0, 1/eth.RMh, 0; 1/eth.clRNh, 0, 0; 0, 0, 1];
    len = fix(sum(wat1(:,1))/ts);
    [imu, avp] = prealloc(len, 7, 10);
    ki = timebar(1, len, 'Trajectory Simulation.');
    for k=1:size(wat1,1)
        lenk = round(wat1(k,1)/ts);  % the lenght at k phase
        wt = wat1(k,3:5)'; at = wat1(k,6:8)';
        ufflag = 0;
        if norm(wt)==0 && norm(at)==0  % uniform phase flag
            ufflag = 1; 
            vnr = a2mat(att)*[0;wat1(k,2);0];  % velocity damping reference
        end
        for j=1:lenk
            sa = sin(att); ca = cos(att);
            si = sa(1); sk = sa(3); ci = ca(1); ck = ca(3); 
            Cnt = [ ck, -ci*sk,  si*sk; 
                    sk,  ci*ck, -si*ck; 
                    0,   si,     ci ];
            att = att + wt*ts;
            Cnb = a2mat(att);   % attitude
            if ufflag==1  % damping
                na = norm(vn-vnr)/ts;  maxa = 0.1;
                if na<maxa,  na=maxa;  end  % max an is limited to maxa/(m/s^2)
                vn1 = vn-damping/na*(vn-vnr);  an = (vn1-vn)/ts;
                vn01 = (vn+vn1)/2;  vn = vn1;
            else
                an = Cnt*at;
                vn1 = vn + an*ts;  vn01 = (vn+vn1)/2;  vn = vn1;  % velocity
            end
            dpos01 = [vn01(2)/eth.RMh;vn01(1)/eth.clRNh;vn01(3)]*ts2;
            eth = earth(pos+dpos01, vn01);
            pos = pos+dpos01*2;      % position
%             eth = earth(pos+Mpv*vn01*ts2, vn01);
%             Mpv = [0, 1/eth.RMh, 0; 1/eth.clRNh, 0, 0; 0, 0, 1];
%             pos = pos+Mpv*vn01*ts;      % position
            phim = m2rv(Cbn_1*Cnb) + (Cbn_1+Cnb')*(eth.wnin*ts2);
%             wm = phim;
            wm = (glv.I33+askew(wm_1/12))^-1*phim;   % gyro increment
%             wm = (glv.I33-askew(wm_1/12))*phim;   % gyro increment
%             dvbm = Cbn_1*qmulv(rv2q(eth.wnin*ts2), (an-eth.gcc)*ts);
            dvbm = Cbn_1*(rv2m(eth.wnin*ts2)*(an-eth.gcc)*ts);
            vm = (glv.I33+askew(wm/2))^-1*dvbm;   % acc increment
%             vm = (glv.I33-askew(wm/2))*dvbm;   % acc increment
            kts = ki*ts;
            avp(ki,:) = [att; vn; pos; kts]';
            imu(ki,:) = [wm; vm; kts]';
            wm_1 = wm; Cbn_1 = Cnb';
            ki = timebar;
        end
    end
    avp(ki:end,:) = []; imu(ki:end,:) = [];
    avp = iatt2c(avp);
    trj = varpack(imu, avp, avp0, wat, ts, repeats);