%%%%%?gps?kalman?%%%%%
clear;
%-------------?參數定義?-----------%
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;??%地球橢球體扁率
b_e=（1-f_e）*a_e;??????%地球橢球短半軸
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??325.73?190.96;2??325.73?220.48;3??325.73?330.17;4??325.73?83.58;??????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????????
??????5??25.73??249.90;6??25.73??352.12;7??25.73??25.25;?8??25.73??124.10;
??????9??85.73??286.20;10?85.73??48.94;?11?85.73??155.08;12?85.73??183.71;
??????13?145.73?312.30;14?145.73?340.93;15?145.73?87.06;?16?145.73?209.81;
??????17?205.73?11.90;?18?205.73?110.76;19?205.73?143.88;20?205.73?246.11;
??????21?265.73?52.42;?22?265.73?165.83;23?265.73?275.52;24?265.73?305.04];
for?j=1:24
????sate（j2）=sate（j2）*pi/180;%升交點赤經
????sate（j3）=sate（j3）*pi/180;%平近點角
end
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（‘E:\work\program\New\Programs\trace.dat‘‘r‘）;
while?1
?????linestring?=?fgets（fid）;
?????if?linestring???????????break;
?????end
????????place=sscanf（linestring‘%*f%f%f%f%f%*[^\n]‘）;
????????gps_longi=place（1）;
????????gps_lati=place（2）;
????????gps_height=place（3）;
????????gps_velo=place（4）;
????????t_u?=?t_u+1????????%?實時顯示用戶運行時間?
????????user（1t_u）=gps_longi;%用戶經緯高信息
????????user（2t_u）=gps_lati;
????????user（3t_u）=gps_height;
????????user（4t_u）=gps_velo;
????????V（1t_u）=user（4t_u）;
??
????????%?用戶在大地坐標系中的經緯度數據經度L緯度B高度H?%