clc;
clear;
close all;
warning off;
addpath(genpath(pwd));
%------------- 参数定义 -----------%%%%%% gps kalman zishiying %%%%%
pi=3.1415926;
C=3.0e8; %光速
a=26609e3; %轨道长半轴长,单位已经换算为 m
e=0.006; %轨道的偏心率
i_0=55*pi/180; %基准时间t_0的轨道倾角
a_e=6378137; %地球椭球的长半径
f_e=1/298.257223563; %地球椭球体扁率
e_2=2*f_e-f_e^2; %GPS参考椭球第一偏心率的平方
E0=10; %定义的仰角比较值
mu=3.986008e14; %开普勒常数,单位为m3/s2
w_ie=7.292115147e-5; %地球自转平均角速率,单位rad/s
% 卫星轨道参数矩阵epoch:2007-04-01 14:21:46,第一列卫星标号1~30,第二列升交点赤经W_0,第三列平近点角距M_0 %
sate=[1 12.4664 313.6181;2 13.2561 99.1277;3 10.4480 41.9519;4 12.4727 289.5653;5 11.6077 116.5486;6 10.5432 209.4507;
7 65.1275 73.1881;8 66.9823 101.9770;9 68.3454 286.8979;10 73.1479 200.4810;11 70.2154 77.1527;
12 125.5712 295.1060;13 128.3618 285.5386;14 131.8240 123.7737;15 131.3074 44.5724;16 130.3997 73.6977;
17 186.9655 97.3078;18 188.4401 98.7626;19 184.9619 319.1793;20 194.1995 55.7345;21 190.8460 172.0155;
22 253.5063 46.1491;23 251.3094 346.3001;24 241.0610 333.6671;25 252.3879 163.3319;26 250.2394 231.0366;
27 311.8862 334.8484;28 309.8682 287.5291;29 312.8828 146.8152;30 313.3067 94.0084];
t_0=0; %星历的参考历元
a3=a^3;
n=sqrt(mu/a3) % n=(2*pi)/T=sqrt(mu/a3),应用了开普勒第三定律
k=1;
i=1;
A_i=1;
r=1;
T=1;
%
t_u=0;
t_uu0=500; % 用户运行起始时间
%---------------------- 初始位置 ---------------%
%----------------------------------------------------------------------------------------%
fid = fopen('trace1.dat','r');
while 1
linestring = fgets(fid);
if linestring < 0
break;
end
place=sscanf(linestring,'%*f%f%f%f%*[^\n]');
gps_longi=place(1);
gps_lati=place(2);
gps_height=place(3);
t_u = t_u+1 % 实时显示用户运行时间
user(1,t_u)=gps_longi;%用户经纬高信息
user(2,t_u)=gps_lati;
user(3,t_u)=gps_height;
% 用户在大地坐标系中的经纬度数据,经度L,纬度B,高度H %
user1(1,t_u)=user(1,t_u)*pi/180;%完成弧度转换
user1(2,t_u)=user(2,t_u)*pi/180;
L=user1(1,t_u);
B=user1(2,t_u);
H=user(3,t_u);
% 计算椭球的卯酉圈曲率半径N
W=sqrt(1-e_2*sin(B)^2);
N=a_e/W;
% 将用户在大地坐标系下的坐标转换为地球坐标系的空间直角坐标[xp,yp,zp]
xp(1,t_u)=(N+H)*cos(B)*cos(L);
yp(1,t_u)=(N+H)*cos(B)*sin(L);
zp(1,t_u)=(N*(1-e_2)+H)*sin(B);
% 求系数阵h
h(1,1)=-sin(B)*cos(L);h(1,2)=-sin(B)*sin(L);h(1,3)=cos(B);
h(2,1)=-sin(L); h(2,2)=cos(L); h(2,3)=0;
h(3,1)=cos(B)*cos(L); h(3,2)=cos(B)*sin(L); h(3,3)=sin(B);
t_k=t_uu0+t_u; % 找到对应于用户运行的时刻的卫星所在的位置,用户打第t_u个点时,时间为t_uu0+t_u
q=t_k; % 各个矩阵的行数表示量
t(t_u)=t_u;
j=1; % 卫星标号
sum_s(t_u,1)=0; % 求 q 时刻的卫星数目矩阵
while j<=30 %各个矩阵的列数表示量
M_k(q,j)=sate(j,3)+n*(t_k-t_0);
Et_1(q,j)=M_k(q,j);
t_end=1;
while(t_end)
Et(q,j)=M_k(q,j)+e*sin(Et_1(q,j));
delta_E(q,j)=Et(q,j)-Et_1(q,j);
Et_1(q,j)=Et(q,j);
if abs(delta_E(q,j))<=1.0e-6
E_k(q,j)=Et(q,j);
t_end=0;
end
end
%-------------- 求真近点角 f 的值,并进行象限判断 -----------%
A=cos(E_k(q,j))-e; %分母一定是是大于0的数,所以只取分子来做判断
B=sqrt(1-e^2)*sin(E_k(q,j));
if (A==0)
f(q,j)=pi/2;
elseif (B==0)
f(q,j)=pi;
else
f(q,j)=atan(abs(B/A));
if ((B>0)&(A<0))
f(q,j)=pi-f(q,j);
elseif ((B<0)&(A<0))
f(q,j)=pi+f(q,j);
elseif ((B<0)&(A>0))
f(q,j)=2*pi-f(q,j);
end
end
u_k(q,j)=f(q,j);
r_k(q,j)=a*(1-e*cos(E_k(q,j)));
i_k(q,j)=i_0;
L_k(q,j)=sate(j,2)-w_ie*(t_k);
x_k(q,j)=r_k(q,j)*cos(u_k(q,j))*cos(L_k(q,j))-r_k(q,j)*sin(u_k(q,j))*sin(L_k(q,j))*cos(i_k(q,j));
y_k(q,j)=r_k(q,j)*cos(u_k(q,j))*sin(L_k(q,j))+r_k(q,j)*sin(u_k(q,j))*cos(L_k(q,j))*cos(i_k(q,j));
z_k(q,j)=r_k(q,j)*sin(u_k(q,j))*sin(i_k(q,j));
%---计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 -----%
delta_x(q,j)=x_k(q,j)-xp(1,t_u);
delta_y(q,j)=y_k(q,j)-yp(1,t_u);
delta_z(q,j)=z_k(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);
d_star(j)=5*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(q,j)=E_deg(q,j);
if ele(q,j)>=E0
ele(q,j)=1;
if r~=q
i=1;
r=q;
end
s_n(q,i)=j; % j :可见星标号
%-----------%
if s_n(q,i)~=0 % 将可见星提取出来
sd(i)=s_n(q,i);% sd ;可见星标号阵
% -地心坐标系下站星的几何距离 R - %
R=(x_k(q,sd(i))-xp(1,t_u))^2 + (y_k(q,sd(i))-yp(1,t_u))^2 + (z_k(q,sd(i))-zp(1,t_u))^2;
R=sqrt(R);
%------------------------------以下要产生伪距rou------------------------------%
%d_T(t_u)=0;
%d_T(t_u)=100000+randn(1);%-仿真给出接收机在各个时刻的钟差,即折合的距离误差-%
d_T(t_u)=5*randn(1);
%---------------------------------------%
rou(q,i)=R+d_T(t_u)+d_star(sd(i)); % 带误差的伪距
%---------------------------------------------------------------------------%
end
%-----------%
i=i+1;
else
ele(q,j)=0;
end
sum_s(t_u,1)=sum_s(t_u,1)+ele(q,j);
j=j+1;
end
end
% end
%
disp('=================以下开始定位计算,利用递推法=======================');
%-------用户总的运行时间
t_user=t_u;
%*--------------------------matrix defined ------------------------*/
P =diag([25000,25000,25000,1000,1000,1000,0,0,0,90000,900]);
%P =diag([25000,25000,25000,4,4,4,4500000000000,150000000]);
h0=9.4*1e-20; h_1=1.8*1e-19; h_2=3.8*1e-21;
Qt11 = h0*T/2+2*h_1*T^2+2*pi^2*h_2*T^3/3; Qt