📄 position_gg.m
字号:
%R(q,i)=rou(q,i); % 不加任何误差时的伪距R
R_1(q,i)=rou(q,i)+d_T(t_u)+d_star(sd(i)); % 带误差的伪距
%---------------------------------------------------------------------------%
end
%-----------%
i=i+1;
else
ele_1(q,j)=0;
end
sum_s_1(q,1)=sum_s_1(q,1)+ele_1(q,j);
j=j+1;
end
%
sum_s_2(q,1)=0; % 求 q 时刻的卫星数目矩阵
j=1; % 卫星标号
i=1;
while j<=30 %各个矩阵的列数表示量
M_k(q,j)=sate_2(j,3)+n_2*(t_k-t_0);
r_k(q,j)=a_2;
%卫星在轨道直角坐标系中的坐标
gui_1(q,j)=r_k(q,j)*cos(M_k(q,j));
gui_2(q,j)=r_k(q,j)*sin(M_k(q,j));
gui_3(q,j)=0;
%天球坐标系中坐标
xk_2(q,j)=gui_1(q,j)*cos(sate_2(j,2))-gui_2(q,j)*sin(sate_2(j,2))*cos(i_0_galileo);
yk_2(q,j)=gui_1(q,j)*sin(sate_2(j,2))+gui_2(q,j)*cos(sate_2(j,2))*cos(i_0_galileo);
zk_2(q,j)=gui_2(q,j)*sin(i_0_galileo);
%---计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 --%
delta_x(q,j)=xk_2(q,j)-xp(1,t_u);
delta_y(q,j)=yk_2(q,j)-yp(1,t_u);
delta_z(q,j)=zk_2(q,j)-zp(1,t_u);
%求卫星在 % 站心坐标系下 % 的坐标
X_sta(q,j)=h(1,1)*delta_x(q,j)+h(1,2)*delta_y(q,j)+h(1,3)*delta_z(q,j);
Y_sta(q,j)=h(2,1)*delta_x(q,j)+h(2,2)*delta_y(q,j)+h(2,3)*delta_z(q,j);
Z_sta(q,j)=h(3,1)*delta_x(q,j)+h(3,2)*delta_y(q,j)+h(3,3)*delta_z(q,j);
%---------------------给出对应各颗卫星的星历误差---------------------%
%d_star(j)=0;
d_star(j)=50+randn(1);
%--------------------------------------------------------------------%
E_deno(q,j)=X_sta(q,j)^2+Y_sta(q,j)^2;
E_deno(q,j)=sqrt(E_deno(q,j));
if E_deno(q,j)==0
E_rad(q,j)=pi/2;
E_deg(q,j)=90;
else
E_rad(q,j)=atan(Z_sta(q,j)/E_deno(q,j));
E_deg(q,j)=E_rad(q,j)*180/pi;
end
%-- 开始高度角比较 ,E0 为给定的高度角,判断可见星 --%
ele_2(q,j)=E_deg(q,j);
if ele_2(q,j)>=E0
ele_2(q,j)=1;
if r~=q
i=1;
r=q;
end
s_n_2(q,i)=j; % j :可见星标号
%-----------%
if s_n_2(q,i)~=0 % 将可见星提取出来
sd(i)=s_n_2(q,i);% sd ;可见星标号阵
% -地心坐标系下站星的几何距离 rou - %
rou(q,i)=(xk_2(q,sd(i))-xp(1,t_u))^2 + (yk_2(q,sd(i))-yp(1,t_u))^2 + (zk_2(q,sd(i))-zp(1,t_u))^2;
rou(q,i)=sqrt(rou(q,i));
%------------------------------以下要产生伪距R------------------------------%
%d_T(t_u)=0;
d_T(t_u)=100000+randn(1);%-仿真给出接收机在各个时刻的钟差,即折合的距离误差-%
%---------------------------------------%
%R(q,i)=rou(q,i); % 不加任何误差时的伪距R
R_2(q,i)=rou(q,i)+d_T(t_u)+d_star(sd(i)); % 带误差的伪距
%---------------------------------------------------------------------------%
end
%-----------%
i=i+1;
else
ele_2(q,j)=0;
end
sum_s_2(q,1)=sum_s_2(q,1)+ele_2(q,j);
j=j+1;
end
%
end
end
%
disp('===============以下开始定位计算,利用递推法==============');
%-------初始点的近似值
xp_ini(:,1)=[0,0,0,0]';
%xp_ini(:,1)=[-2.6061e+006,4.7375e+006,3.3831e+006,0]';
%-------用户总的运行时间
t_user=t_u;
%----------------------------------------------------------------------------------------%
for t_u=1:t_user %用户运行的总点数
q=t_uu0+t_u;
turn_num=0;
t_end=1;
while(t_end)
kk=sum_s_1(q,1);%gps可见星个数
A_i=1;
for i=1:kk
sd(i)=s_n_1(q,i);% sd ;可见星标号阵
% -地心坐标系下站星的几何距离 rou - %
rou(q,i)=sqrt((xk_1(q,sd(i))-xp_ini(1,t_u))^2 + (yk_1(q,sd(i))-xp_ini(2,t_u))^2 + (zk_1(q,sd(i))-xp_ini(3,t_u))^2);
%-----求解用户位置变化量 ---%
% 求A,V=AX+L %
a_11(sd(i),q)=(xk_1(q,sd(i))-xp_ini(1,t_u))/rou(q,i);a_12(sd(i),q)=(yk_1(q,sd(i))-xp_ini(2,t_u))/rou(q,i);
a_13(sd(i),q)=(zk_1(q,sd(i))-xp_ini(3,t_u))/rou(q,i);a_14(sd(i),q)=1;
A_rou_1(i,:,q)=[-a_11(sd(i),q),-a_12(sd(i),q),-a_13(sd(i),q),a_14(sd(i),q)];
L_a_1(i,1,q)=rou(q,i)+xp_ini(4,t_u)-R_1(q,i);%残差修正，见课本 P84 公式 4-34
end
%%%%%%%%%%%%%%%%
kk=sum_s_2(q,1);%galileo可见星个数
A_i=1;
for i=1:kk
sd(i)=s_n_2(q,i);% sd ;可见星标号阵
% -地心坐标系下站星的几何距离 rou - %
rou(q,i)=sqrt((xk_2(q,sd(i))-xp_ini(1,t_u))^2 + (yk_2(q,sd(i))-xp_ini(2,t_u))^2 + (zk_2(q,sd(i))-xp_ini(3,t_u))^2);
%-----求解用户位置变化量 ---%
% 求A,V=AX+L %
a_21(sd(i),q)=(xk_2(q,sd(i))-xp_ini(1,t_u))/rou(q,i);a_22(sd(i),q)=(yk_2(q,sd(i))-xp_ini(2,t_u))/rou(q,i);
a_23(sd(i),q)=(zk_2(q,sd(i))-xp_ini(3,t_u))/rou(q,i);a_24(sd(i),q)=1;
A_rou_2(i,:,q)=[-a_21(sd(i),q),-a_22(sd(i),q),-a_23(sd(i),q),a_24(sd(i),q)];
L_a_2(i,1,q)=rou(q,i)+xp_ini(4,t_u)-R_2(q,i);%残差修正，见课本 P84 公式 4-34
end
%
A_rou=[A_rou_1(:,:,q);A_rou_2(:,:,q)];
A_rou_1(:,:,q)=zeros;A_rou_2(:,:,q)=zeros;%对其清零，以防干扰下次的运算结果。
L_a=[L_a_1(:,1,q);L_a_2(:,1,q)];
L_a_1(:,1,q)=zeros; L_a_2(:,1,q)=zeros;
%-----------------计算修正值--------------------%
delta_user(:,1,q)=-inv(A_rou'*A_rou)*A_rou'*L_a;
%-修正初始近似值，将迭代数 -加- 到初始值中作修正-%
xp_ini(:,t_u)=xp_ini(:,t_u)+delta_user(:,1,q);
turn_num=turn_num+1;
%----------计算递推终止判断变量 judge----------%
judge(t_u,turn_num)=sqrt(delta_user(1,1,q)^2+delta_user(2,1,q)^2+delta_user(3,1,q)^2);
if judge(t_u,turn_num)<0.1 %判断条件满足否？
x_end(:,t_u)=xp_ini(:,t_u);%得到递推终了值
t_end=0;
% ---计算PDOP值--%
delta_user1(:,:,q)=inv(A_rou'*A_rou);
PDOP(1,t_u)=delta_user1(1,1,q)+delta_user1(2,2,q)+delta_user1(3,3,q);
PDOP(1,t_u)=sqrt(PDOP(1,t_u));
end
end
xp_ini(:,t_u+1)=x_end(:,t_u);
end
%-----对比定位前后的坐标，做差----%
delta1_p_end(1,:)=abs(xp(1,:)-x_end(1,:));
delta2_p_end(1,:)=abs(yp(1,:)-x_end(2,:));
delta3_p_end(1,:)=abs(zp(1,:)-x_end(3,:));
%
%mm=mean(abs(x_end(4,:)));%求时钟误差的平均值
mm=mean(x_end(4,:));%求时钟误差的平均值
%-----------------------------------各个时刻递推完毕-------------------------------------%
disp('============Now ready to plot==============');
%figure(1)%经纬度下用户路线图
%plot(user(1,:),user(2,:),'r.');
%axis equal;
%grid;
figure(2)
plot3(xp(1,:),yp(1,:),zp(1,:),'r',x_end(1,:),x_end(2,:),x_end(3,:),'b.');
grid on;
figure(3)
plot(xp(1,:),yp(1,:),'r',xp(1,1),yp(1,1),'bO',xp(1,t_user),yp(1,t_user),'b*',...
x_end(1,:),x_end(2,:),'b.',x_end(1,1),x_end(2,1),'rO',x_end(1,t_user),x_end(2,t_user),'r*');
%axis equal;
grid;
legend('红线为用户地球坐标系的空间直角坐标','蓝色圆圈为其起始点','蓝色*为其终止点',...
'蓝色点线为递推定位后的用户路线图','红色圆圈为其起始点','红色*为其终止点');
figure(4) % 递推定位前后的用户位置坐标差
subplot(3,1,1)
plot(t,delta1_p_end(1,:),'b');
grid;
subplot(3,1,2)
plot(t,delta2_p_end(1,:),'b');
grid;
subplot(3,1,3)
plot(t,delta3_p_end(1,:),'b');
grid;
figure(5)
plot(t,PDOP(1,:),'b.');
grid;
figure(10)
plot(t,x_end(4,:),'b');
grid;⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -