MATLAB读取bag包实现经纬度到xyz坐标系转换

MATLAB读取bag包实现经纬度到xyz坐标系转换

ros录制的bag包分析数据不方便，在MATLAB中处理可以很直观的显示。
首先在MATLAB读入bag包的所有数据：

bag=rosbag(filepath) ;

然后搜索所需要的数据的数据类型，并读出数据：

geometry_message=select(bag,'MessageType','message_type_name')  ;
data=readMessages(geometry_message)  ;

此时，所需要的数据存在cell中，可以随时进行处理。

我需要把bag包中的经纬度转换为xyz坐标。
首先需要确认经纬度中是弧度还是角度，其转换公式为：
角度转弧度 = π/180×角度
弧度变角度 = 180/π×弧度
通过一下公式进行转换：

x=r*sin(lat)*cos(lon);
y=r*sin(lat)*sin(lon);
% z=r*cos(lat);

r是地球坐标，6371Km.
如果还有不明白的，可以在我的博客下载相应的MATLAB代码。