GPS接受机中的卡尔曼滤波MATLAB代码

%%%%% 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 < 0
          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 %