激光雷达与组合惯导联合标定--方案二（matlab）
目录
-
- 原理
- matlab代码
- 对程序的改进
【说明】：
网上关于雷达与惯导联合标定的方法有很多，大多数方法在实际操作起来并不太方便，为了获得矩阵变换关系有的还用到了slam建图、神经网络等方法。
之前学习了一下lidar_align标定雷达与惯导，但实际操作效果并不太理想，可能是自己采集的数据不太满足要求，所以自己尝试着通过坐标系转换关系建立方程并用matlab求解的，精度不一定满足要求，可能有很多不足之处，仅供参考。
【lidar_align标定参考链接： https://blog.csdn.net/tfb760/article/details/108581092】
方案一（靠特征点计算）：https://blog.csdn.net/xx970829/article/details/115072158
原理
按照方案一，在实际采集数据时比较困难，纸箱或车等标志物在点云中都不够明显，若要良好效果需要以一栋小平房的一角来做角点，于是提出了方案二：通过室外楼梯或台阶一条明显的点云线在地图坐标系中建立一条空间直线方程，在不同时刻，不同帧之间对应雷达坐标系中的点云线在地图坐标系中在同一条空间直线Ax+By+Cz+D=0上。
然后通过无人机搭载激光雷达和组合惯导来采集该标志线，通过点云可以看到该标志线，而且可以得到该标志线对应角点的在雷达坐标系中的坐标（x1,y1,z1）,时间戳对齐后，可以提取出该帧对应的四元数（qw,qx,qy,qz）及gps经度，纬度，海拔。
通过四元数可以算出相对地图坐标系的旋转矩阵，gps可以得到相对地图坐标系的平移，可以得到变换矩阵M1。
根据对比方案一，多了一个未知数（共16个未知数），标志线上每个点都可以带入空间直线方程，所以至少需要16个点。
上图公式可知：（a,b,c）表示点云中线特征上某一点在地图 坐标系下空间直线上的坐标，M0（4x4）是不变的，M1和点云中线特征上的点云坐标在雷达坐标系中的位置是可以知道的，那么如果知道多组点云（至少16组）数据，其实就可以求出空间直线方程和M0（4x4)了。
matlab代码
（下面的代码中只写了取四帧不同位姿下的点云，每帧4个雷达点）
[使用时，将提取到的数据保存到一个param.txt文件里，按四元数（w x y z）、经度、纬度、海拔、点云（x,y,z）的顺序保存（每帧写4个点云坐标），param.txt文件与这几个.m文件放在同一目录下。运行imu_lidar.m文件，将生成一个result.txt文件来保存结果]
imu_lidar.m
clc
syms a b c d t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz;
m0=[0,0,0,0,0,1,0.1,0,1,0,0.1,1,0,0,0.2,0];
x=fsolve(@funline,m0,optimset('fsolve'));
a=x(1); b=x(2); c=x(3);
t11=x(4); t12=x(5); t13=x(6); tx=x(7);
t21=x(8); t22=x(9); t23=x(10);ty=x(11);
t31=x(12);t32=x(13);t33=x(14);tz=x(15);
R=[t11,t12,t13;t21,t22,t23;t31,t32,t33];
q=dcm2quat(R);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 旋转矩阵 ');
fprintf('%6.8f %6.8f %6.8f \n',t11,t12,t13);
fprintf('%6.8f %6.8f %6.8f \n',t21,t22,t23);
fprintf('%6.8f %6.8f %6.8f \n',t31,t32,t33);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 平移 ');
fprintf('%6.8f %6.8f %6.8f \n',tx,ty,tz);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 四元数 ');
fprintf('%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
%----------------------------------------------------------------------------------------------------------
T=fopen('.\result.txt','w');%创建文件
fprintf(T,'旋转矩阵： \n');
fprintf(T,'%6.8f %6.8f %6.8f \n',t11,t12,t13);
fprintf(T,'%6.8f %6.8f %6.8f \n',t21,t22,t23);
fprintf(T,'%6.8f %6.8f %6.8f \n',t31,t32,t33);
fprintf(T,'平移： \n');
fprintf(T,'%6.8f %6.8f %6.8f \n',tx,ty,tz);
fprintf(T,'四元数： \n');
fprintf('%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
fclose(T);
funline.m
function P=funline(x)
%打开txt数据
F=importdata('.\param.txt');
%四元数转旋转矩阵
R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);
R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);
R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);
%经度 纬度 海拔
gx_1=F(1,5); gy_1=F(1,6); gz_1=F(1,7);
gx_2=F(2,5); gy_2=F(2,6); gz_2=F(2,7);
gx_3=F(3,5); gy_3=F(3,6); gz_3=F(3,7);
gx_4=F(4,5); gy_4=F(4,6); gz_4=F(4,7);
% 点云
x11=F(1,8); y11=F(1,9); z11=F(1,10); x12=F(1,11); y12=F(1,12); z12=F(1,13); x13=F(1,14); y13=F(1,15); z13=F(1,16); x14=F(1,17); y14=F(1,18); z14=F(1,19);
x21=F(2,8); y21=F(2,9); z21=F(2,10); x22=F(2,11); y22=F(2,12); z22=F(2,13); x23=F(2,14); y23=F(2,15); z23=F(2,16); x24=F(2,17); y24=F(2,18); z24=F(2,19);
x31=F(3,8); y31=F(3,9); z31=F(3,10); x32=F(3,11); y32=F(3,12); z32=F(3,13); x33=F(3,14); y33=F(3,15); z33=F(3,16); x34=F(3,17); y34=F(3,18); z34=F(3,19);
x41=F(4,8); y41=F(4,9); z41=F(4,10); x42=F(4,11); y42=F(4,12); z42=F(4,13); x43=F(4,14); y43=F(4,15); z43=F(4,16); x44=F(4,17); y44=F(4,18); z44=F(4,19);
%经纬度转墨卡托
[gx11,gy11]=gpstoMercator(gx_1,gy_1);
[gx22,gy22]=gpstoMercator(gx_2,gy_2);
[gx33,gy33]=gpstoMercator(gx_3,gy_3);
[gx44,gy44]=gpstoMercator(gx_4,gy_4);
%纬度差为飞机的x方向位移，经度差为飞机的y方向位移
gx1=0; gy1=0; gz1=0;
gx2=gy11-gy22; gy2=gx11-gx22; gz2=gz_1-gz_2;
gx3=gy11-gy33; gy3=gx11-gx33; gz3=gz_1-gz_3;
gx4=gy11-gy44; gy4=gx11-gx44; gz4=gz_1-gz_4;
%第一帧旋转赋值
m1_11=R1(1,1); m1_12=R1(1,2); m1_13=R1(1,3);
m1_21=R1(2,1); m1_22=R1(2,2); m1_23=R1(2,3);
m1_31=R1(3,1); m1_32=R1(3,2); m1_33=R1(3,3);
%第2帧旋转赋值
m2_11=R2(1,1); m2_12=R2(1,2); m2_13=R2(1,3);
m2_21=R2(2,1); m2_22=R2(2,2); m2_23=R2(2,3);
m2_31=R2(3,1); m2_32=R2(3,2); m2_33=R2(3,3);
%第3帧旋转赋值
m3_11=R3(1,1); m3_12=R3(1,2); m3_13=R3(1,3);
m3_21=R3(2,1); m3_22=R3(2,2); m3_23=R3(2,3);
m3_31=R3(3,1); m3_32=R3(3,2); m3_33=R3(3,3);
%第4帧旋转赋值
m4_11=R4(1,1); m4_12=R4(1,2); m4_13=R4(1,3);
m4_21=R4(2,1); m4_22=R4(2,2); m4_23=R4(2,3);
m4_31=R4(3,1); m4_32=R4(3,2); m4_33=R4(3,3);
% t11=x(4)t12=x(5)t13=x(6);tx=x(7); t21=x(8)t22=x(9)t23=x(10) ty=x(11); t31=x(12)t32=x(13)t33=x(14);tz=x(15);
% c--------c--------c---------------------c---------c--------c-----------------------c----------c---------c---------------------------------c--------c--------c---------------------c---------c--------c-----------------------c----------c---------c-------------------------------c--------c--------c---------------------c-------c---------c------------------------c--------c---------c--------------------
P=[ x(1)*(m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1)+x(2)*(m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1)+x(3)*(m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1)+x(16);
x(1)*(m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1)+x(2)*(m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1)+x(3)*(m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1)+x(16);
x(1)*(m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1)+x(2)*(m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1)+x(3)*(m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1)+x(16);
x(1)*(m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1)+x(2)*(m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1)+x(3)*(m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1)+x(16);
x(1)*(m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2)+x(2)*(m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2)+x(3)*(m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2)+x(16);
x(1)*(m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2)+x(2)*(m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2)+x(3)*(m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2)+x(16);
x(1)*(m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2)+x(2)*(m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2)+x(3)*(m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2)+x(16);
x(1)*(m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2)+x(2)*(m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2)+x(3)*(m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2)+x(16);
x(1)*(m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3)+x(2)*(m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3)+x(3)*(m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3)+x(16);
x(1)*(m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3)+x(2)*(m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3)+x(3)*(m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3)+x(16);
x(1)*(m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3)+x(2)*(m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3)+x(3)*(m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3)+x(16);
x(1)*(m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3)+x(2)*(m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3)+x(3)*(m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3)+x(16);
x(1)*(m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4)+x(2)*(m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4)+x(3)*(m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4)+x(16);
x(1)*(m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4)+x(2)*(m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4)+x(3)*(m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4)+x(16);
x(1)*(m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4)+x(2)*(m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4)+x(3)*(m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4)+x(16);
x(1)*(m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4)+x(2)*(m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4)+x(3)*(m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4)+x(16)];
end
gpstoMercator.m
function [jing,wei] = gpstoMercator(j,w )
jing = j * 20037508.34 / 180;
ly = log(tand((90+ w)*pi/360))/(pi/180);
wei = ly *20037508.34/180;
end
对程序的改进
imu_lidar_fmincon.m
clc
syms a b c d t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz;
%**********************************************************************************************
% 输入参数
%*********************************************************************************************
%设初始值
imux=0; %空间点数据在惯导坐标系的大致位置
imuy=0.5;
imuz=0.75;
txx=0.18;%手测位移
tyy=0;
tzz=0.24;
l_i_z=0.2;%手测沿惯导坐标系z轴平移
l_i_x=0.1;%手测沿惯导坐标系x轴平移
l_i_z2=0.14;%手测沿惯导坐标系z轴平移(到雷达中心高度)
aa=40;%沿惯导坐标系y轴旋转角度
%--------------------------------------------------------------------------
[A,B,C,D]= funintial(imux,imuy,imuz);
h=0;
% [ | | | |
m0=[A,B,C,h,h,1,txx,h,1,h,tyy,1,h,h,tzz,D];
A = [];b = [];
Aeq = []; beq = [];
vlb = []; vub = [];
options = optimoptions('fmincon','Algorithm','sqp','Display','iter','ConstraintTolerance',1e-12);
[x,fval]=fmincon(@funcon,m0,A, b, Aeq, beq, vlb, vub,@fcon,options);
Rr=imutolidar(l_i_z,l_i_x,l_i_z2,aa);
a=x(1); b=x(2); c=x(3);
t11=x(4); t12=x(5); t13=x(6); tx=x(7);
t21=x(8); t22=x(9); t23=x(10);ty=x(11);
t31=x(12);t32=x(13);t33=x(14);tz=x(15);
R=[t11,t12,t13;t21,t22,t23;t31,t32,t33];
q=dcm2quat(R);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 原始矩阵 ');
disp(Rr);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 旋转矩阵 ');
fprintf('%6.8f %6.8f %6.8f \n',t11,t12,t13);
fprintf('%6.8f %6.8f %6.8f \n',t21,t22,t23);
fprintf('%6.8f %6.8f %6.8f \n',t31,t32,t33);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 平移 ');
fprintf('%6.8f %6.8f %6.8f \n',tx,ty,tz);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 四元数 ');
fprintf('%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
%----------------------------------------------------------------------------------------------------------
T=fopen('.\result.txt','w');%创建文件
fprintf(T,'旋转矩阵： \n');
fprintf(T,'%6.8f %6.8f %6.8f \n',t11,t12,t13);
fprintf(T,'%6.8f %6.8f %6.8f \n',t21,t22,t23);
fprintf(T,'%6.8f %6.8f %6.8f \n',t31,t32,t33);
fprintf(T,'平移： \n');
fprintf(T,'%6.8f %6.8f %6.8f \n',tx,ty,tz);
fprintf(T,'四元数： \n');
fprintf(T,'%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
fclose(T);
fcon.m
%***********************************************************
% 约束条件
%***********************************************************
function [c,ceq]=fcon(x) %构造约束函数 c=[]<0; ceq= 0
%-----------------------------------------------------------
% 输入参数
%-----------------------------------------------------------
tx=0.18; %手测位移
ty=0;
tz=0.24;
l_i_z=0.2; %手测沿惯导坐标系z轴平移
l_i_x=0.1; %手测沿惯导坐标系x轴平移
l_i_z2=0.14; %手测沿惯导坐标系z轴平移(到雷达中心高度)
a=40; %手测沿惯导坐标系y轴旋转角度
%----------------------------------
L=0.3; %点偏离平面范围（单位m）
Lt=0.05; %偏离手测位移
Lr=0.05; %偏离旋转
La=50; %偏离平面系数范围
%-----------------------------------
imux=0; %算空间平面时空间点数据在惯导坐标系的大致位置
imuy=0.5;
imuz=0.75;
%----------------------------------
R=imutolidar(l_i_z,l_i_x,l_i_z2,a);
[A,B,C,D]= funintial(imux,imuy,imuz);
%-----------------------------------
%打开txt数据
F=importdata('.\data.txt');
%四元数转旋转矩阵
R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);
R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);
R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);
%经度 纬度 海拔
gx_1=F(1,5); gy_1=F(1,6); gz_1=F(1,7);
gx_2=F(2,5); gy_2=F(2,6); gz_2=F(2,7);
gx_3=F(3,5); gy_3=F(3,6); gz_3=F(3,7);
gx_4=F(4,5); gy_4=F(4,6); gz_4=F(4,7);
% 点云
x11=F(1,8); y11=F(1,9); z11=F(1,10); x12=F(1,11); y12=F(1,12); z12=F(1,13); x13=F(1,14); y13=F(1,15); z13=F(1,16); x14=F(1,17); y14=F(1,18); z14=F(1,19);
x21=F(2,8); y21=F(2,9); z21=F(2,10); x22=F(2,11); y22=F(2,12); z22=F(2,13); x23=F(2,14); y23=F(2,15); z23=F(2,16); x24=F(2,17); y24=F(2,18); z24=F(2,19);
x31=F(3,8); y31=F(3,9); z31=F(3,10); x32=F(3,11); y32=F(3,12); z32=F(3,13); x33=F(3,14); y33=F(3,15); z33=F(3,16); x34=F(3,17); y34=F(3,18); z34=F(3,19);
x41=F(4,8); y41=F(4,9); z41=F(4,10); x42=F(4,11); y42=F(4,12); z42=F(4,13); x43=F(4,14); y43=F(4,15); z43=F(4,16); x44=F(4,17); y44=F(4,18); z44=F(4,19);
%经纬度转墨卡托
[gx11,gy11]=gpstoMercator(gx_1,gy_1);
[gx22,gy22]=gpstoMercator(gx_2,gy_2);
[gx33,gy33]=gpstoMercator(gx_3,gy_3);
[gx44,gy44]=gpstoMercator(gx_4,gy_4);
%纬度差为飞机的x方向位移，经度差为飞机的y方向位移
gx1=0; gy1=0; gz1=0;
gx2=gy22-gy11; gy2=gx22-gx11; gz2=gz_2-gz_1;
gx3=gy33-gy11; gy3=gx33-gx11; gz3=gz_3-gz_1;
gx4=gy44-gy11; gy4=gx44-gx11; gz4=gz_4-gz_1;
%第一帧旋转赋值
m1_11=R1(1,1); m1_12=R1(1,2); m1_13=R1(1,3);
m1_21=R1(2,1); m1_22=R1(2,2); m1_23=R1(2,3);
m1_31=R1(3,1); m1_32=R1(3,2); m1_33=R1(3,3);
%第2帧旋转赋值
m2_11=R2(1,1); m2_12=R2(1,2); m2_13=R2(1,3);
m2_21=R2(2,1); m2_22=R2(2,2); m2_23=R2(2,3);
m2_31=R2(3,1); m2_32=R2(3,2); m2_33=R2(3,3);
%第3帧旋转赋值
m3_11=R3(1,1); m3_12=R3(1,2); m3_13=R3(1,3);
m3_21=R3(2,1); m3_22=R3(2,2); m3_23=R3(2,3);
m3_31=R3(3,1); m3_32=R3(3,2); m3_33=R3(3,3);
%第4帧旋转赋值
m4_11=R4(1,1); m4_12=R4(1,2); m4_13=R4(1,3);
m4_21=R4(2,1); m4_22=R4(2,2); m4_23=R4(2,3);
m4_31=R4(3,1); m4_32=R4(3,2); m4_33=R4(3,3);
f1=x(1)*(m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1)+x(2)*(m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1)+x(3)*(m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1)+x(16);
f2=x(1)*(m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1)+x(2)*(m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1)+x(3)*(m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1)+x(16);
f3=x(1)*(m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1)+x(2)*(m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1)+x(3)*(m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1)+x(16);
f4=x(1)*(m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1)+x(2)*(m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1)+x(3)*(m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1)+x(16);
f5=x(1)*(m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2)+x(2)*(m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2)+x(3)*(m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2)+x(16);
f6=x(1)*(m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2)+x(2)*(m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2)+x(3)*(m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2)+x(16);
f7=x(1)*(m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2)+x(2)*(m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2)+x(3)*(m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2)+x(16);
f8=x(1)*(m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2)+x(2)*(m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2)+x(3)*(m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2)+x(16);
f9=x(1)*(m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3)+x(2)*(m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3)+x(3)*(m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3)+x(16);
f10=x(1)*(m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3)+x(2)*(m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3)+x(3)*(m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3)+x(16);
f11=x(1)*(m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3)+x(2)*(m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3)+x(3)*(m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3)+x(16);
f12=x(1)*(m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3)+x(2)*(m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3)+x(3)*(m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3)+x(16);
f13=x(1)*(m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4)+x(2)*(m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4)+x(3)*(m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4)+x(16);
f14=x(1)*(m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4)+x(2)*(m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4)+x(3)*(m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4)+x(16);
f15=x(1)*(m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4)+x(2)*(m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4)+x(3)*(m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4)+x(16);
f16=x(1)*(m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4)+x(2)*(m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4)+x(3)*(m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4)+x(16);
c=[x(1)-A-La;x(2)-B-La;x(3)-C-La;x(4)-R(1,1)-Lr;x(5)-R(1,2)-Lr;x(6)-R(1,3)-Lr;x(7)-tx-Lt;x(8)-R(2,1)-Lr;x(9)-R(2,2)-Lr;x(10)-R(2,3)-Lr;x(11)-ty-Lt;x(12)-R(3,1)-Lr;x(13)-R(3,2)-Lr;x(14)-R(3,3)-Lr;x(15)-tz-Lt;x(16)-D-La;-x(1)+A-La;-x(2)+B-La;-x(3)+C-La;-x(4)+R(1,1)-Lr;-x(5)+R(1,2)-Lr;-x(6)+R(1,3)-Lr;-x(7)+tx-Lt;-x(8)+R(2,1)-Lr;-x(9)+R(2,2)-Lr;-x(10)+R(2,3)-Lr;-x(11)+ty-Lt;-x(12)+R(3,1)-Lr;-x(13)+R(3,2)-Lr;-x(14)+R(3,3)-Lr;-x(15)+tz-Lt;-x(16)+D-La;f1-L;f2-L;f3-L;f4-L;f5-L;f6-L;f7-L;f8-L;f9-L;f10-L;f11-L;f12-L;f13-L;f14-L;f15-L;f16-L;L-f1;L-f2;L-f3;L-f4;L-f5;L-f6;L-f7;L-f8;L-f9;L-f10;L-f11;L-f12;L-f13;L-f14;L-f15;L-f16];
ceq=[];
end
funcon.m
%***********************************************************
% 目标函数
%**********************************************************
function F=funcon(x) %构造约束函数 c=[]<0; ceq= 0
%打开txt数据
F=importdata('.\data.txt');
%四元数转旋转矩阵
R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);
R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);
R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);
%经度 纬度 海拔
gx_1=F(1,5); gy_1=F(1,6); gz_1=F(1,7);
gx_2=F(2,5); gy_2=F(2,6); gz_2=F(2,7);
gx_3=F(3,5); gy_3=F(3,6); gz_3=F(3,7);
gx_4=F(4,5); gy_4=F(4,6); gz_4=F(4,7);
% 点云
x11=F(1,8); y11=F(1,9); z11=F(1,10); x12=F(1,11); y12=F(1,12); z12=F(1,13); x13=F(1,14); y13=F(1,15); z13=F(1,16); x14=F(1,17); y14=F(1,18); z14=F(1,19);
x21=F(2,8); y21=F(2,9); z21=F(2,10); x22=F(2,11); y22=F(2,12); z22=F(2,13); x23=F(2,14); y23=F(2,15); z23=F(2,16); x24=F(2,17); y24=F(2,18); z24=F(2,19);
x31=F(3,8); y31=F(3,9); z31=F(3,10); x32=F(3,11); y32=F(3,12); z32=F(3,13); x33=F(3,14); y33=F(3,15); z33=F(3,16); x34=F(3,17); y34=F(3,18); z34=F(3,19);
x41=F(4,8); y41=F(4,9); z41=F(4,10); x42=F(4,11); y42=F(4,12); z42=F(4,13); x43=F(4,14); y43=F(4,15); z43=F(4,16); x44=F(4,17); y44=F(4,18); z44=F(4,19);
%经纬度转墨卡托
[gx11,gy11]=gpstoMercator(gx_1,gy_1);
[gx22,gy22]=gpstoMercator(gx_2,gy_2);
[gx33,gy33]=gpstoMercator(gx_3,gy_3);
[gx44,gy44]=gpstoMercator(gx_4,gy_4);
%纬度差为飞机的x方向位移，经度差为飞机的y方向位移
gx1=0; gy1=0; gz1=0;
gx2=gy22-gy11; gy2=gx22-gx11; gz2=gz_2-gz_1;
gx3=gy33-gy11; gy3=gx33-gx11; gz3=gz_3-gz_1;
gx4=gy44-gy11; gy4=gx44-gx11; gz4=gz_4-gz_1;
%第一帧旋转赋值
m1_11=R1(1,1); m1_12=R1(1,2); m1_13=R1(1,3);
m1_21=R1(2,1); m1_22=R1(2,2); m1_23=R1(2,3);
m1_31=R1(3,1); m1_32=R1(3,2); m1_33=R1(3,3);
%第2帧旋转赋值
m2_11=R2(1,1); m2_12=R2(1,2); m2_13=R2(1,3);
m2_21=R2(2,1); m2_22=R2(2,2); m2_23=R2(2,3);
m2_31=R2(3,1); m2_32=R2(3,2); m2_33=R2(3,3);
%第3帧旋转赋值
m3_11=R3(1,1); m3_12=R3(1,2); m3_13=R3(1,3);
m3_21=R3(2,1); m3_22=R3(2,2); m3_23=R3(2,3);
m3_31=R3(3,1); m3_32=R3(3,2); m3_33=R3(3,3);
%第4帧旋转赋值
m4_11=R4(1,1); m4_12=R4(1,2); m4_13=R4(1,3);
m4_21=R4(2,1); m4_22=R4(2,2); m4_23=R4(2,3);
m4_31=R4(3,1); m4_32=R4(3,2); m4_33=R4(3,3);
%点坐标
f1x=m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1;
f1y=m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1;
f1z=m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1;
f2x=m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1;
f2y=m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1;
f2z=m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1;
f3x=m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1;
f3y=m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1;
f3z=m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1;
f4x=m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1;
f4y=m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1;
f4z=m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1;
f5x=m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2;
f5y=m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2;
f5z=m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2;
f6x=m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2;
f6y=m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2;
f6z=m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2;
f7x=m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2;
f7y=m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2;
f7z=m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2;
f8x=m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2;
f8y=m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2;
f8z=m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2;
f9x=m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3;
f9y=m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3;
f9z=m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3;
f10x=m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3;
f10y=m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3;
f10z=m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3;
f11x=m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3;
f11y=m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3;
f11z=m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3;
f12x=m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3;
f12y=m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3;
f12z=m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3;
f13x=m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4;
f13y=m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4;
f13z=m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4;
f14x=m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4;
f14y=m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4;
f14z=m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4;
f15x=m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4;
f15y=m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4;
f15z=m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4;
f16x=m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4;
f16y=m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4;
f16z=m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4;
% d=|ax0+by0+cz0+d|/√(a?+b?+c?)
d1=abs(f1x*x(1)+f1y*x(2)+f1z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d2=abs(f2x*x(1)+f2y*x(2)+f2z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d3=abs(f3x*x(1)+f3y*x(2)+f3z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d4=abs(f4x*x(1)+f4y*x(2)+f4z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d5=abs(f5x*x(1)+f5y*x(2)+f5z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d6=abs(f6x*x(1)+f6y*x(2)+f6z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d7=abs(f7x*x(1)+f7y*x(2)+f7z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d8=abs(f8x*x(1)+f8y*x(2)+f8z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d9=abs(f9x*x(1)+f9y*x(2)+f9z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d10=abs(f10x*x(1)+f10y*x(2)+f10z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d11=abs(f11x*x(1)+f11y*x(2)+f11z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d12=abs(f12x*x(1)+f12y*x(2)+f12z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d13=abs(f13x*x(1)+f13y*x(2)+f13z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d14=abs(f14x*x(1)+f14y*x(2)+f14z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d15=abs(f15x*x(1)+f15y*x(2)+f15z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
d16=abs(f16x*x(1)+f16y*x(2)+f16z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
F=d1+d2+d3+d4+d5+d6+d7+d8+d9+d10+d11+d12+d13+d14+d15+d16;
%-------------------------------------------------------------------------------------------------------------------------------------------------
end
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
[明天验证后再补充]
小编之后演算了一下，发现初始值设置对结果影响比较大，最近在忙其他问题，可能最近没有时间解决，仅提供思路参考。。。
【参考方案三】：https://blog.csdn.net/xx970829/article/details/115911534?utm_medium=distribute.pc_relevant.none-task-blog-2defaultbaidujs_title~default-0.control&spm=1001.2101.3001.4242