Gvins运行结果处理，时间戳对齐并与RTK真值对比 （matalb代码）

        最近在学习Gvins，准备自己用matlab处理一下结果

        以sports_field数据集为例，运行Gvins的程序后，将Gvins的导航结果保存到CSV文件中，得到如下图所示的数据格式

        其中第一列为时间戳，第三四五列是ecef坐标，首先要做的就是将gvins在ecef坐标系下的坐标与RTK真值做差，然后将差值转换到ENU坐标系下。

通过对gnss_comm和toolkit文件夹进行编译和运行可以获得rtk真值的文件

RTK的格式为：

gnss_ts_ns,ecef_px,ecef_py,ecef_pz,enu_vx,enu_vy,enu_vz,fix_type,valid_fix,diff_soln,carr_soln

首先对Gvins结果和RTK真值结果进行读取，下面以Gvins文件读取为例，RTK文件读取同理

function [Gvins]=gvins_xyz(BasfilPath)
BasfilPath={'gvins_gnss_result.csv'};
for k=1:length(BasfilPath)
path=BasfilPath{k};
fid=fopen(path);  %打开观测数据文件路径
    j=0;
    while ~feof(fid) %循环  文件指针fid到达文件末尾时，该表达式值为‘假’，否则为‘真’
        line=fgetl(fid);
        line=strrep(line,',',' ');
        data4line=sscanf(line(1:end),'%e');  
        j=j+1;
        T = round(data4line(2)/100000000); 
        X = data4line(3);
        Y = data4line(4);
        Z = data4line(5);
      Gvins(j,1)= T;
      Gvins(j,2) = X;
      Gvins(j,3) = Y;
      Gvins(j,4) = Z;
    end
    status=fclose(fid);  %关闭文件
end
end

T = round(data4line(2)/100000000);   是将时间戳保留到秒的下一位，然后进行取整

BasfilPath={'gvins_gnss_result.csv'};   %读文件
[Gvins]=gvins_xyz(BasfilPath);
BasfilPath={'rtk.csv'};
[RTK_xyz]=rtk_xyz(BasfilPath);

t_rtk_xyz= RTK_xyz(:,1);   %时间戳对齐
t_gvins = Gvins(:,1);
t_com = intersect(t_rtk_xyz,t_gvins);
for i=1:length(t_com)
r=find(t_rtk_xyz==t_com(i));
index_RTK(i)=r;
g=find(t_gvins==t_com(i));
index_GVINS(i)=g;
end
RTK_USE = RTK_xyz(index_RTK,:);
gvins_USE = Gvins(index_GVINS,:);

for j = 1 : length(index_RTK)
   X(j) = gvins_USE(j,2) - RTK_USE(j,2);
   Y(j)= gvins_USE(j,3) - RTK_USE(j,3);
   Z(j)= gvins_USE(j,4) - RTK_USE(j,4);
  [E(j),N(j),U(j)]=XYZ2ENU(X(j),Y(j),Z(j));
  dENU(j,1)= E(j);
  dENU(j,2)= N(j);
  dENU(j,3)= U(j);
  
  
end
   Ture_T=round(t_rtk_xyz(14880,1)/10)-(t_rtk_xyz(1,1)/10);

figure
subplot(3,1,1)
plot(E,'r','LineWidth',1)
ylabel('E_e/m');
axis([0 Ture_T -inf inf]);
hold on
subplot(3,1,2)
plot(N,'g','LineWidth',1)
ylabel('N_e/m');
axis([0 Ture_T -inf inf]);
hold on
subplot(3,1,3)
plot(U,'y','LineWidth',1)
ylabel('U_e/m');
xlabel('时间 /t');
axis([0 Ture_T -inf inf]);

其中涉及到ECEF坐标系转换到ENU坐标系的matalb代码，有时间再写一个坐标系转换的博客，未完待续.......

最终得到Gvins结果与RTK真值的误差对比图