Demo entry 6626196

matlab

   

Submitted by anonymous on Jun 23, 2017 at 14:31
Language: Matlab. Code size: 2.5 kB.

format long
E1=eye(3);
E2=-eye(3);
O=zeros(3,3);
b1=[E1 O O E2;O O E1 E2;O E1 E2 O;O E1 O E2;E1 E2 O O];
BC1=[270.457;-503.208;1879.923] ;
BC2=[83.497;714.153;-1029.199];
BC3=[1405.531;-175.157;1171.380];
BC4=[1489.013;536.030;142.218];
BC5=[-1218.561;-1039.227;1737.720];
BJ1=[-1974538.215; 4590064.705;3953140.500];
BJ2=[-1973319.652; 4591103.903;3951402.778];
BJ3=[-1974725.180; 4591282.078;3950231.408];
WG1=[-1974638.734; 4590014.819;3953144.924];
BC1=[270.457;-503.208;1879.923];
WG4=WG1-BC1;
BJ4=WG4+(BJ1-WG1);
A=BJ1-BJ4;
l=[BC1-A];
B1=[diag([0 0 0])+[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]-[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]'];
B1=[B1 A];
A=BJ3-BJ4;
l=[l;BC2-A];
B2=[diag([0 0 0])+[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]-[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]'];
B2=[B2 A];
A=BJ2-BJ3;
l=[l;BC3-A];
B3=[diag([0 0 0])+[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]-[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]'];
B3=[B3 A];
A=BJ2-BJ4;
l=[l;BC4-A];
B4=[diag([0 0 0])+[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]-[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]'];
B4=[B4 A];
A=BJ1-BJ2;
l(1:3,1)=zeros(3,1);
l=1000*[l;BC5-A];
B5=[diag([0 0 0])+[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]-[0 -A(3,1) A(2,1);0 0  -A(1,1);0 0 0]'];
B5=[B5 A];
b2=1000*[B1;B2;B3;B4;B5];
b3=[E1;E1;E1;E1;E1]; %七参数需要附加一组平移参数
B=[b1 b3 b2];
Q1=diag([1.04 6.34 6.04])+[0 -2.40 -2.32;0 0 5.90;0 0 0]+ [0 -2.40 -2.32;0 0 5.90;0 0 0]';
Q2=diag([9.66 51.95 43.24])+[0 -21.75 -19.71;0 0 46.33;0 0 0]+ [0 -21.75 -19.71;0 0 46.33;0 0 0]';
Q3=diag([1.20 6.89 4.82])+[0 -2.64 -2.17;0 0 5.48;0 0 0]+ [0 -2.64 -2.17;0 0 5.48;0 0 0]';
Q4=diag([0.59 3.36 3.02])+[0 -1.33 -1.25;0 0 3.07;0 0 0]+ [0 -1.33 -1.25;0 0 3.07;0 0 0]';
Q5=diag([0.23 1.34 1.01])+[0 -0.51 -0.44;0 0 1.11;0 0 0]+ [0 -0.51 -0.44;0 0 1.11;0 0 0]';
Q=blkdiag(Q1,Q2,Q3,Q4,Q5);
P=inv(Q);
%构造限制条件矩阵G以及求出x值,其坐标改正值单位为m
c=[O O [0;0;0]];%由于加入了b3,因此限制条件矩阵G的构造需要符合矩阵相乘的要求,多添加一组3×3零矩阵
G=[E1 O O O c;O E1 O O c;O O E1 O c];
x=pinv(B'*P*B+G'*G)*B'*P*l;%这里如果用inv,会有报错,说矩阵接近奇异矩阵,所以这里用求pinv来解决
v=1/1000*(B*x-l);
V=[BC1;BC2;BC3;BC4;BC5]+v;
x(1:12,1)=x(1:12,1)*1/1000

x =

  1.0e+002 *

  -0.00179382225246
  -0.00125563689001
  -0.00032597154679
   0.00214762734802
   0.00183941057446
  -0.00225424804012
   0.00022213824473
  -0.00224110430542
   0.00176124088041
   0.00174587270698
   0.00761328301789
-0.00851110651410
   0.89562765921548
   5.17072004315618
  -3.11065821763423
   0.00000273833056
   0.00000118242908
  -0.00000487603807
   0.00000000464362

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).