博客

数据准备：

将rosbag包转成TXT文件的命令：

rostopic echo -b 2023-04-01-11-01-25.bag /imu > 1.txt

转换结果如图所示（其中的一帧数据）：

1.MATLAB数据提取

针对文本格式，使用MATLAB提取文本中的四元数、线加速度及角加速度数据，分别存为orientation.txt、acceleration.txt、angle_velocity.txt文件。

使用函数：

• fid = fopen(‘1.txt’,’r’);          %读取源文件
  

• tline = fgets(fid);                %读取文件中的一行
  

• [ln,~,~,n] = sscanf(tline,’%s’,1);
  

• tline1=tline(k1:end);      %截取本行所需要的数据部分
  

sscanf

函数：



[




A




,




n




,




errmsg




,




nextindex




] = sscanf

(

str

,

formatSpec

,

sizeA

)

A：从

str

读取数据，根据

formatSpec

指定的格式对其进行转换，并将结果返回到数组中。        如果

str

是一个包含多行的字符数组，

sscanf

以列顺序读取字符。

n:  返回

sscanf

成功读入

A

的元素数。

errmsg：在

sscanf

无法将所有数据读入

A

时返回包含错误消息的字符向量。

nextindex：返回

str

中紧跟在由

sscanf

扫描的最后一个字符之后的位置的索引。

2.MATLAB浮点型长度设置

1）打开MATLAB上的设置

2） 在命令行窗口中，设置数值格式为long, 并修改制表符大小为8位。即可修改MATLAB记录及打印数值的小数点后位数。

默认长度：

修改后长度：

在写入数据时，需要确定数据格式（确定数据位数）：

fprintf(fid,’%15.12f     ‘,angle_velocity.w_x(i));  % %15.12f 表示共占15位，小数点后记录12位

文件写入代码：

fid = fopen('orientation_3.txt','w');
for i = 1 : length(ori_Var.o_x)   %%浮点数保留12位小数
    fprintf(fid,'%8.2f     ',time_stamp(i)); 
    fprintf(fid,'%15.12f     ',ori_Var.o_x(i)); 
    fprintf(fid,'%15.12f     ',ori_Var.o_y(i)); 
    fprintf(fid,'%15.12f     ',ori_Var.o_z(i)); 
    fprintf(fid,'%15.12f   \n',ori_Var.o_w(i));
end
fclose(fid);

MATLAB代码：

clear all;
close all;

time_stamp = [];        %%数据以100Hz的频率记录
time = 0;
time_interval = 0.01;

ori_Var.o_x = [];       %四元素数据
ori_Var.o_y = [];
ori_Var.o_z = [];
ori_Var.o_w = [];

angle_velocity.w_x = []; %角速度数据
angle_velocity.w_y = [];
angle_velocity.w_z = [];

acceleration.a_x = [];   %加速度数据
acceleration.a_y = [];
acceleration.a_z = [];

fid = fopen('1.txt','r');   %读取文本文件

tline = fgets(fid);
while tline ~= -1
    [ln,~,~,n] = sscanf(tline,'%s',1);
    if ln=="orientation:"           %匹配数据种类
        tline = fgets(fid);         %读取文本中的一行数据
        k1 = strfind(tline, 'x');
        if k1>0
            k1=k1+2;                %跳过数据开头的 x: ,两个字符
            tline1=tline(k1:end);
            x = sscanf(tline1,'%f',1);    %数据由字符型转换为浮点型
                                          %MATLAB浮点数默认为小数点后4位，其他长度需自行设定
            ori_Var.o_x = [ori_Var.o_x;x];%将数据累计到数组中，方便操作
        end
        tline = fgets(fid);
        k1 = strfind(tline, 'y');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            y = sscanf(tline1,'%f',1);
            ori_Var.o_y = [ori_Var.o_y;y];
        end        
        tline = fgets(fid);
        k1 = strfind(tline, 'z');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            z = sscanf(tline1,'%f',1);
            ori_Var.o_z = [ori_Var.o_z;z];
        end
        tline = fgets(fid);
        k1 = strfind(tline, 'w');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            w = sscanf(tline1,'%f',1);
            ori_Var.o_w = [ori_Var.o_w;w];
        end
        time = time + time_interval;
        time_stamp = [time_stamp;time];
    end
    
    %%角速度数据
    if ln=="angular_velocity:"
        tline = fgets(fid);
        k1 = strfind(tline, 'x');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            x = sscanf(tline1,'%f',1);
            angle_velocity.w_x = [angle_velocity.w_x;x];
        end
        tline = fgets(fid);
        k1 = strfind(tline, 'y');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            y = sscanf(tline1,'%f',1);
            angle_velocity.w_y = [angle_velocity.w_y;y];
        end
        tline = fgets(fid);
        k1 = strfind(tline, 'z');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            z = sscanf(tline1,'%f',1);
            angle_velocity.w_z = [angle_velocity.w_z;z];
        end
    end
    
    %%加速度数据      
    if ln=="linear_acceleration:"
        tline = fgets(fid);
        k1 = strfind(tline, 'x');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            x = sscanf(tline1,'%f',1);
            acceleration.a_x = [acceleration.a_x;x];
        end
        tline = fgets(fid);
        k1 = strfind(tline, 'y');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            y = sscanf(tline1,'%f',1);
            acceleration.a_y = [acceleration.a_y;y];
        end
        tline = fgets(fid);
        k1 = strfind(tline, 'z');
        if k1>0
            k1=k1+2;
            tline1=tline(k1:end);
            z = sscanf(tline1,'%f',1);
            acceleration.a_z = [acceleration.a_z;z];
        end
    end
    
    tline = fgets(fid);
end

fclose(fid);
disp("read data well! "); 



fid = fopen('orientation_1.txt','w');     %%新建或打开文件，进行写操作
for i = 1 : length(ori_Var.o_x)           %%浮点数保留12位小数，时间保留两位小数
    fprintf(fid,'%8.2f     ',time_stamp(i)); 
    fprintf(fid,'%15.12f     ',ori_Var.o_x(i)); 
    fprintf(fid,'%15.12f     ',ori_Var.o_y(i)); 
    fprintf(fid,'%15.12f     ',ori_Var.o_z(i)); 
    fprintf(fid,'%15.12f   \n',ori_Var.o_w(i));
end
fclose(fid);                              %%关闭文件
disp("orientation write well."); 

fid = fopen('angle_velocity_1.txt','w');
for i = 1 : length(angle_velocity.w_x)   %%浮点数保留12位小数
    fprintf(fid,'%8.2f     ',time_stamp(i));
    fprintf(fid,'%15.12f     ',angle_velocity.w_x(i)); 
    fprintf(fid,'%15.12f     ',angle_velocity.w_y(i)); 
    fprintf(fid,'%15.12f   \n',angle_velocity.w_z(i)); 
end
fclose(fid);
disp("angle_velocity write well."); 

fid = fopen('acceleration_1.txt','w');
for i = 1 : length(acceleration.a_x)   %%浮点数保留12位小数
    fprintf(fid,'%8.2f     ',time_stamp(i));
    fprintf(fid,'%15.12f     ',acceleration.a_x(i)); 
    fprintf(fid,'%15.12f     ',acceleration.a_y(i)); 
    fprintf(fid,'%15.12f   \n',acceleration.a_z(i)); 
end
fclose(fid);
disp("acceleration write well."); 

文件及数据已上传至阿里云盘：

阿里云盘分享