误差理论与测量平差实习（间接平差求棱镜位置，并画误差椭圆）

TOC](目录)

代码

clear 
clc
load("angle.mat")
load("length.mat")
load("dis.mat")
load("length.mat")
load("uwbpos.mat")
format long g;
%测站坐标 313621.086 499580.854 
%棱镜坐标 313644.411 499595.598
P=eye(12);
x_zhan=313621.086;y_zhan=499580.854;x_jing0=313644.411;y_jing0=499595.598;
alpha=atan((x_jing0-x_zhan)/(y_jing0-y_zhan)); %起始方位角
jiao=angle(7);
for j=6:-1:1
X0(j)=x_zhan+dis(j)*sin(jiao+alpha);
Y0(j)=y_zhan+dis(j)*cos(jiao+alpha);
jiao=jiao+angle(j);
end  
 s=(X0(6)-x_zhan)^2+(Y0(6)-y_zhan)^2;   
 D1(7)=(Y0(6)-y_zhan)/s;D2(7)=-(X0(6)-x_zhan)/s;
for i=6:-1:2
    s1=(X0(i-1)-x_zhan)^2+(Y0(i-1)-y_zhan)^2;   %?
    s2=(X0(i)-x_zhan)^2+(Y0(i)-y_zhan)^2;
    D3(i)=(Y0(i-1)-y_zhan)/s1;D4(i)=-(X0(i-1)-x_zhan)/s1;
    D1(i)=-(Y0(i)-y_zhan)/s2;D2(i)=(X0(i)-x_zhan)/s2;
end
s=(X0(1)-x_zhan)^2+(Y0(1)-y_zhan)^2;   
D1(1)=-(Y0(1)-y_zhan)/s;D2(1)=(X0(1)-x_zhan)/s;

B(1,:)=[D1(1),D2(1),0,0,0,0,0,0,0,0,0,0];
B(2,:)=[D3(2),D4(2),D1(2),D2(2),0,0,0,0,0,0,0,0];
B(3,:)=[0,0,D3(3),D4(3),D1(3),D2(3),0,0,0,0,0,0];
B(4,:)=[0,0,0,0,D3(4),D4(4),D1(4),D2(4),0,0,0,0];
B(5,:)=[0,0,0,0,0,0,D3(5),D4(5),D1(5),D2(5),0,0];
B(6,:)=[0,0,0,0,0,0,0,0,D3(6),D4(6),D1(6),D2(6)];
B(7,:)=[0,0,0,0,0,0,0,0,0,0,D1(7),D2(7)];

for k=1:6
    s=sqrt((X0(k)-x_zhan)^2+(Y0(k)-y_zhan)^2);
    C1(k)=(X0(k)-x_zhan)/s; 
    C2(k)=(Y0(k)-y_zhan)/s;
end
for m=1:5
    s=sqrt((X0(m+1)-X0(m))^2+(Y0(m+1)-Y0(m))^2);
    E1(m)=-(X0(m+1)-X0(m))/s;
    E2(m)=-(Y0(m+1)-Y0(m))/s;
    E3(m)=(X0(m+1)-X0(m))/s;
    E4(m)=(Y0(m+1)-Y0(m))/s;
end
B(8,:)=[C1(1),C2(1),0,0,0,0,0,0,0,0,0,0];
B(9,:)=[0,0,C1(2),C2(2),0,0,0,0,0,0,0,0];
B(10,:)=[0,0,0,0,C1(3),C2(3),0,0,0,0,0,0];
B(11,:)=[0,0,0,0,0,0,C1(4),C2(4),0,0,0,0];
B(12,:)=[0,0,0,0,0,0,0,0,C1(5),C2(5),0,0];
B(13,:)=[0,0,0,0,0,0,0,0,0,0,C1(6),C2(6)];
B(14,:)=[E1(1),E2(1),E3(1),E4(1),0,0,0,0,0,0,0,0];
B(15,:)=[0,0,E1(2),E2(2),E3(2),E4(2),0,0,0,0,0,0];
B(16,:)=[0,0,0,0,E1(3),E2(3),E3(3),E4(3),0,0,0,0];
B(17,:)=[0,0,0,0,0,0,E1(4),E2(4),E3(4),E3(5),0,0];
B(18,:)=[0,0,0,0,0,0,0,0,E1(5),E2(5),E3(5),E4(5)];

L=[angle;dis';length'];i=0;k=0;m=0;
l1(7)=atan((X0(6)-x_zhan)/(Y0(6)-y_zhan))-alpha+pi;
for i=6:-1:2 
    l1(i)=(atan((X0(i-1)-x_zhan)/(Y0(i-1)-y_zhan))-atan((X0(i)-x_zhan)/(Y0(i)-y_zhan)));
end
l1(1)=-atan((X0(1)-x_zhan)/(Y0(1)-y_zhan))+alpha;   %角
l1(3)=l1(3)+pi;
for k=1:6
    l2(k)=sqrt((X0(k)-x_zhan)^2+(Y0(k)-y_zhan)^2)     %距离d
end
for m=1:5
    l3(m)=sqrt((X0(m+1)-X0(m))^2+(Y0(m+1)-Y0(m))^2);     %长度l
end
l=[l1';l2';l3'];
l=L-l;
N_bb=B'*B;
x_hat=inv(N_bb)*B'*l;
p_0=[X0(1),Y0(1),X0(2),Y0(2),X0(3),Y0(3),X0(4),Y0(4),X0(5),Y0(5),X0(6),Y0(6)];
X=p_0'+x_hat;
V=B*x_hat-l;
L_hat=L+V;
%精度评定
sigma0=sqrt((V'*V)/6);
Q_xx=inv(N_bb);
D_xx=sigma0^2*Q_xx;
figure(1)
for i=1:12
    sigmax(i)=D_xx(i,i);
    heng1(i)=i;
end
plot(heng1,sigmax)
title('坐标参数中误差折线图');
Q_LL=B*inv(N_bb)*B';
D_LL=Q_LL*sigma0;
for i=1:7
    sigmab(i)=D_LL(i,i);
    heng2(i)=i;
end
figure(2)
plot(heng2,sigmab)
title('观测值被β中误差折线图');
xlabel('观测值角度β')
ylabel('中误差')
for i=8:13
    sigmad(i-7)=D_LL(i,i);
    heng3(i-7)=i-7;
end
figure(3)
plot(heng3,sigmad)
xlabel('观测值距离d')
ylabel('中误差')
title('观测值被d中误差折线图');
for i=14:18
    sigmal(i-13)=D_LL(i,i);
    heng4(i-13)=i-13;
end
figure(4)
plot(heng4,sigmal)
xlabel('观测值距离l')
ylabel('中误差')
title('观测值l中误差折线图');
%画位置图
figure(5)
heng = [X0(1),X0(2),X0(3),X0(4),X0(5),X0(6)];
zong=[Y0(1),Y0(2),Y0(3),Y0(4),Y0(5),Y0(6)];
plot(heng, zong);
hold on
for i = 1:6
    x = [x_zhan, heng(i)];
    y = [y_zhan, zong(i)];
    plot(x, y,'-o')
end
plot([x_zhan, x_jing0], [y_zhan, y_jing0], '-*')
title('棱镜位置图');
legend('棱镜位置', '棱镜1', '棱镜2', '棱镜3', '棱镜4', '棱镜5', '棱镜6', '初始棱镜')

K=sqrt(((Q_xx(1,1)-Q_xx(2,2))^2)+4*Q_xx(1,2)^2);
Q_EE=0.5*(Q_xx(1,1)+Q_xx(2,2)+K);
E=sqrt(sigma0^2*Q_EE);
Q_FF=0.5*(Q_xx(1,1)+Q_xx(2,2)-K);
F=sqrt(sigma0^2*Q_FF);
fai_E=atan((Q_EE-Q_xx(1,1))/Q_xx(1,2));
fai_F=atan((Q_FF-Q_xx(1,1))/Q_xx(1,2));

for i=1:2:11

K=sqrt(((Q_xx(i,i)-Q_xx(i+1,i+1))^2)+4*Q_xx(i,i+1)^2);
Q_EE=0.5*(Q_xx(i,i)+Q_xx(i+1,i+1)+K);
E=sqrt(sigma0^2*Q_EE);
Q_FF=0.5*(Q_xx(i,i)+Q_xx(i+1,i+1)-K);
F=sqrt(sigma0^2*Q_FF);
fai_E=atan((Q_EE-Q_xx(i,i))/Q_xx(i,i+1));
fai_F=atan((Q_FF-Q_xx(i,i))/Q_xx(i,i+1));


changzhou = E;
duanzhou = F;
chushijiao = fai_E;
jiao = linspace(0, 2*pi, 100);
x = changzhou/2 * cos(jiao) * cos(chushijiao) - duanzhou/2 * sin(jiao) * sin(chushijiao);
y = changzhou/2 * cos(jiao) * sin(chushijiao) + duanzhou/2 * sin(jiao) * cos(chushijiao);

figure(6);
hold on;
plot(x, y, 'b', 'LineWidth', 1.5);
axis equal;
grid on;
title('误差椭圆');
xlabel('X轴');
ylabel('Y轴');