关于工具箱
gpssimu是生成GPS的位置和速度信息的函数,在psins240101\base\base1目录下
本文所述的代码需要基于PSINS工具箱,工具箱的讲解:
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 s s i m u ( t r j . a v p , [ 1 ; 1 ; 1 ] , [ 1 ; 1 ; 1 ] ) ; out = gpssimu(trj.avp,[1;1;1],[1;1;1]); out=gpssimu(trj.avp,[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
函数解析
第一部分,补全输入量:
第二部分,计算时间戳,对其更新频率:
第三部分,添加误差: