function E = fitness1(a,b,c,tx,ty,tz)
syms a b c tx ty tz%%%%不知道定义这个对不对？？？前3个旋转矩阵参数，后三个平移矩阵参数
%Q最近点，PP测量点，xcl等数据点太多，没全传
Q=[-62.5420000000000,-16.2660000000000,368;-62.5950000000000,-16.2690000000000,368;-62.6390000000000,-16.2570000000000,368;-62.6780000000000,-16.2350000000000,368;-62.7100000000000,-16.2040000000000,368;-62.7360000000000,-16.1540000000000,368];
xcl=[-62.5407000000000;-62.5957000000000;-62.6412000000000;-62.6814000000000;-62.7146000000000;-62.7428000000000];
ycl=[-16.2755000000000;-16.2749000000000;-16.2625000000000;-16.2396000000000;-16.2079000000000;-16.1562000000000];
zcl=[368.001000000000;368.004000000000;368.004000000000;368.003000000000;368.005000000000;368;368.002000000000];
PP=[xcl(1:231),ycl(1:231),zcl(1:231)]’;
R=[cos(b).*cos(c),-cos(b).*sin(c),-sin(b);sin(a).*sin(b).*cos(c)+cos(a).*sin(c),-sin(a).*sin(b).*sin(c)+cos(a).*cos(c),-sin(a).*cos(b);-cos(a).*sin(b).*cos(c)+sin(a).*sin(c),cos(a).*sin(b).*sin(c)+sin(a).*cos(c),cos(a).*cos(b)];
T=[tx,ty,tz]’;
E=0；
for i=1:231
E=E+(norm(QQ(:,i)-R*PP(:,i)-T))^2;
end
———————————————————
%%%%龚老师的粒子群算法
function [xm,fv] = PSO(fitness,N,c1,c2,w,M,D)
format long;
%——初始化种群的个体————
for i=1:N
for j=1:D
x(i,j)=randn; %随机初始化位置
v(i,j)=randn; %随机初始化速度
end
end
%——先计算各个粒子的适应度，并初始化Pi和Pg———————-
for i=1:N
p(i)=fitness(x(i,:));
y(i,:)=x(i,:);
end
pg = x(N,:); %Pg为全局最优
for i=1:(N-1)
if fitness(x(i,:))
pg=x(i,:);
end
end
%——进入主要循环，按照公式依次迭代————
for t=1:M
for i=1:N
v(i,:)=w*v(i,:)+c1*rand*(y(i,:)-x(i,:))+c2*rand*(pg-x(i,:));
x(i,:)=x(i,:)+v(i,:);
if fitness(x(i,:))
p(i)=fitness(x(i,:));
y(i,:)=x(i,:);
end
if p(i)
pg=y(i,:);
end
end
Pbest(t)=fitness(pg);
end
xm = pg’;
fv = fitness(pg);