误差理论与测量平差实习（计算棱镜位置）

嘀嗒小蜗牛

已于 2024-07-23 22:28:08 修改

阅读量310

点赞数 6

分类专栏：误差理论与测量平差实习文章标签：数据库 matlab 线性代数

于 2024-03-29 22:30:18 首次发布

本文链接：https://blog.csdn.net/weixin_67362258/article/details/137158030

版权

误差理论与测量平差实习专栏收录该内容

3 篇文章 0 订阅

订阅专栏

TOC](目录)

题目要求

由已知点求出方位角，通过平差后边长角度计算基站坐标。
请添加图片描述

在这里插入图片描述

代码

clear
clc
load("angle.mat")
load("length.mat")
load("dis.mat")
load("length.mat")
load("uwbpos.mat")
format long g;
%一个圆周条件，五个图形条件
G=zeros(6,18);
W=zeros(1,6);
p=eye(18);
W(1)=angle(1)+angle(2)+angle(3)+angle(4)+angle(5)+angle(6)+angle(7)-2*pi;
for i=1:5
D1(i)=2*dis(i)-2*dis(i+1)*cos(angle(i+1)); %β角
D2(i)=2*dis(i+1)-2*dis(i)*cos(angle(i+1)); %d边
D3(i)=2*dis(i)*dis(i+1)*sin(angle(i+1))/206265; %d边
D4(i)=2*length(i); %l边
W(i+1)=[dis(i)^2+dis(i+1)^2-length(i)^2-2*dis(i)*dis(i+1)*cos(angle(i+1))];
end
%七个角，6条内边，5个外边
G(1,:)=[1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0];
G(2,:)=[0,D3(1),0,0,0,0,0,D1(1),D2(1),0,0,0,0,-D4(1),0,0,0,0];
G(3,:)=[0,0,D3(2),0,0,0,0,0,D1(2),D2(2),0,0,0,0,-D4(2),0,0,0];
G(4,:)=[0,0,0,D3(3),0,0,0,0,0,D1(3),D2(3),0,0,0,0,-D4(3),0,0];
G(5,:)=[0,0,0,0,D3(4),0,0,0,0,0,D1(4),D2(4),0,0,0,0,-D4(4),0];
G(6,:)=[0,0,0,0,0,D3(5),0,0,0,0,0,D1(5),D2(5),0,0,0,0,-D4(5)];
W=W';
Naa=G*G';
K=-inv(Naa)*W;
V=G'*K;
X0=[angle;dis';length'];
X_hat=X0+V; %平差值
%闭合差检验
g=X_hat(1);
for k=1:6
    g=g+X_hat(k+1);
end
%精度评定
F=G*V+W'
sigma_0=sqrt((V'*p*V)/6);
Q_VV=p'*G'*inv(Naa)*G*p';
Q_LLhat=p'-Q_VV;
D_LLhat=sigma_0^2*Q_LLhat;;
for i=1:18
    D_LL(i)=sqrt(D_LLhat(i,i));   %每个观测值的中误差
end
figure(1)   %画观测值中误差条形统计图
x = 1:1:18;
b = bar(x, D_LL, 0.9) %0.6表示条形图宽度，可修改
xlabel('观测值')
ylabel('中误差')
xtips1 = b.XEndPoints;
ytips1 = b.YEndPoints; 
labels1 = string(b.YData); 
text(xtips1,ytips1,labels1,'HorizontalAlignment','center',...
    'VerticalAlignment','bottom')
%测站坐标 313621.086 499580.854 
%棱镜坐标 313644.411 499595.598
x_zhan0=313621.086;y_zhan0=499580.854;x_jing0=313644.411;y_jing0=499595.598;
alpha=atan((x_jing0-x_zhan0)/(y_jing0-y_zhan0)); %起始方位角
jiao=X_hat(1);
for j=1:6
jing_p(j,1)=x_zhan0-X_hat(7+j)*sin(jiao-alpha);
jing_p(j,2)=y_zhan0+X_hat(7+j)*cos(jiao-alpha);
jiao=jiao+X_hat(j+1);
end
ro=206232;
A1 = [-sin(X_hat(1)-alpha), -X_hat(8)*cos(X_hat(1)-alpha)/ro]; 
A2 = [cos(X_hat(1)-alpha), -X_hat(8)*sin(X_hat(1)-alpha)/ro];
B1 = [-sin(X_hat(1)+X_hat(2)-alpha), -X_hat(9)*cos(X_hat(1)+X_hat(2)-alpha)/ro, -X_hat(9)*cos(X_hat(1)+X_hat(2)-alpha)/ro]; 
B2 = [cos(X_hat(1)+X_hat(2)-alpha), -X_hat(9)*sin(X_hat(1)+X_hat(2)-alpha)/ro, -X_hat(9)*sin(X_hat(1)+X_hat(2)-alpha)/ro];
C1 = [-sin(X_hat(1)+X_hat(2)+X_hat(3)-alpha),-X_hat(10)*cos(X_hat(1)+X_hat(2)+X_hat(3)-alpha)/ro, -X_hat(10)*cos(X_hat(1)+X_hat(2)+X_hat(3)-alpha)/ro, -X_hat(10)*cos(X_hat(1)+X_hat(2)+X_hat(3)-alpha)/ro];
C2 = [cos(X_hat(1)+X_hat(2)+X_hat(3)-alpha),-X_hat(10)*sin(X_hat(1)+X_hat(2)+X_hat(3)-alpha)/ro, -X_hat(10)*sin(X_hat(1)+X_hat(2)+X_hat(3)-alpha)/ro, -X_hat(10)*sin(X_hat(1)+X_hat(2)+X_hat(3)-alpha)/ro];
D1 = [-sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha),-X_hat(11)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro, -X_hat(11)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro,-X_hat(11)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro,-X_hat(11)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro];
D2 = [cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha),-X_hat(11)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro, -X_hat(11)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro,-X_hat(11)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro,-X_hat(11)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)-alpha)/ro];
E1 = [-sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha),-X_hat(12)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro];
E2 = [cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha),-X_hat(12)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro,-X_hat(12)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)-alpha)/ro];
F1 = [-sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha),-X_hat(13)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro];
F2 = [cos(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha),-X_hat(13)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro,-X_hat(13)*sin(X_hat(1)+X_hat(2)+X_hat(3)+X_hat(4)+X_hat(5)+X_hat(6)-alpha)/ro];
M(1,:)=[A1(1),0,0,0,0,0,0,A1(2),0,0,0,0,0,0,0,0,0,0];
M(2,:)=[B1(1),B1(2),0,0,0,0,0,0,B1(3),0,0,0,0,0,0,0,0,0];
M(3,:)=[C1(1),C1(2),C1(3),0,0,0,0,0,0,C1(4),0,0,0,0,0,0,0,0];
M(4,:)=[D1(1),D1(2),D1(3),D1(4),0,0,0,0,0,0,D1(5),0,0,0,0,0,0,0];
M(5,:)=[E1(1),E1(2),E1(3),E1(4),0,0,0,0,0,0,0,E1(6),0,0,0,0,0,0];
M(6,:)=[F1(1),F1(2),F1(3),F1(4),F1(5),F1(6),0,0,0,0,0,0,F1(7),0,0,0,0,0];
sigma_x=sqrt(sigma_0^2*M*Q_LLhat*M');
for z=1:6
    sigma_X(z)=sigma_x(z,z);
end
figure(2)
plot(sigma_X)
N(1,:)=[A2(1),0,0,0,0,0,0,A2(2),0,0,0,0,0,0,0,0,0,0];
N(2,:)=[B2(1),B2(2),0,0,0,0,0,0,B2(3),0,0,0,0,0,0,0,0,0];
N(3,:)=[C2(1),C2(2),C2(3),0,0,0,0,0,0,C2(4),0,0,0,0,0,0,0,0];
N(4,:)=[D2(1),D2(2),D2(3),D2(4),0,0,0,0,0,0,D2(5),0,0,0,0,0,0,0];
N(5,:)=[E2(1),E2(2),E2(3),E2(4),0,0,0,0,0,0,0,E2(6),0,0,0,0,0,0];
N(6,:)=[F2(1),F2(2),F2(3),F2(4),F2(5),F2(6),0,0,0,0,0,0,F2(7),0,0,0,0,0];
sigma_y=sqrt(sigma_0^2*N*Q_LLhat*N');
for z=1:6
    sigma_Y(z)=sigma_y(z,z);
end
figure(3)
plot(sigma_Y)
figure(4)
heng = jing_p(:,1);
zong = jing_p(:,2);
plot(heng, zong);
hold on
for i = 1:6
    x = [x_zhan0, heng(i)];
    y = [y_zhan0, zong(i)];
    plot(x, y,'-o')
end
plot([x_zhan0, x_jing0], [y_zhan0, y_jing0], '-*')
title('棱镜位置图');
legend('棱镜位置', '棱镜1', '棱镜2', '棱镜3', '棱镜4', '棱镜5', '棱镜6', '初始棱镜')