经纬度与墨卡托之间的转换（matlab）

经纬度转墨卡托
gpstoMercator.m

function [jing,wei]  = gpstoMercator(j,w )
jing = j * 20037508.34 / 180;
    ly = log(tan((90+ w)*pi/360))/(pi/180);
wei =  ly *20037508.34/180;
end

墨卡托转经纬度
MercatorToGps.m

function [jing,wei]  = MercatorToGps(j,w )
jing = j/ 20037508.34 * 180;
   ly= w/ 20037508.34 * 180;
wei = 180 /pi * (2 *atan(exp(ly *pi / 180)) - pi / 2);
end

举例：

%*******************************************************************************
%     根据某点四元数与GPS计算出其雷达坐标系的经纬坐标
%*******************************************************************************

clc
%导入旋转矩阵
l_i_z=0.2;   %手测沿惯导坐标系z轴平移
l_i_x=0.1;   %手测沿惯导坐标系x轴平移
l_i_z2=0.14; %手测沿惯导坐标系z轴平移(到雷达中心高度)
a=40;        %手测沿惯导坐标系y轴旋转角度
R=imutolidar(l_i_z,l_i_x,l_i_z2,a);
%打开txt数据
F=importdata('.\data_3.txt');
%四元数转旋转矩阵   
R0=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
R_g=[R0(1,1) R0(1,2) R0(1,3) 0;R0(2,1) R0(2,2) R0(2,3) 0;R0(3,1) R0(3,2) R0(3,3) 0;0 0 0 1];
%经纬度转墨卡托
[g_jin,g_wei]=gpstoMercator(F(1,6),F(1,5));
%输入点云坐标
a_l=[F(1,8);F(1,9);F(1,10);1];
b_l=[F(1,11);F(1,12);F(1,13);1];
%惯导坐标系中的点云坐标
a_g=R_g*R*a_l;
b_g=R_g*R*b_l;
%变化量
a_jin_m=g_jin+a_g(2,1);
a_wei_m=g_wei+a_g(1,1);
b_jin_m=g_jin+b_g(2,1);
b_wei_m=g_wei+b_g(1,1);
%墨卡托转经纬度
[a_jin,a_wei] = MercatorToGps(a_jin_m,a_wei_m);
[b_jin,b_wei] = MercatorToGps(b_jin_m,b_wei_m);
%保存
T=fopen('.\ABpoint.txt','w');%创建文件
fprintf(T,'%6.10f  %6.10f\n',F(1,6),F(1,5));
fprintf(T,'%6.10f  %6.10f\n',a_jin,a_wei);
fprintf(T,'%6.10f  %6.10f\n',b_jin,b_wei);
fclose(T);

其中建立：imutolidar.m

%***********************************************************
%            惯导坐标系到雷达坐标系
%**********************************************************
function R=imutolidar(l_i_z,l_i_x,l_i_z2,a)

%沿惯导坐标系z轴平移l_i_zm
t_z=[1 0 0 0;0 1 0 0;0 0 1 l_i_z;0 0 0 1];
%绕惯导坐标系y轴逆时针旋转a度
r_y=[cosd(a) 0 sind(a) 0;0 1 0 0;-sind(a) 0 cosd(a) 0;0 0 0 1];
%沿惯导坐标系x轴平移l_i_x
t_x=[1 0 0 l_i_x;0 1 0 0;0 0 1 l_i_z2;0 0 0 1];
%绕惯导坐标系y轴逆时针旋转180度,然后绕z轴逆时针旋转90度 
 r_i_l=[0 0 1 0;0 -1 0 0;1 0 1 0;0 0 0 1];
R=t_z*r_y*t_x*r_i_l;
 
end

测试数据：data_3.txt

0.9044525623	0.0096225319	-0.0185025521	0.4260642231	45.7140581215	126.6285656427	138.3351655314	2.032	2.411	0.4993	1.956	-1.016	0.5088