版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
基于matlab的GPS单点定位程序开发
开发初衷
本人是测绘专业的学生,在初学GPS时遇到了很多代码问题,因此希望这篇文章能够帮助广大的测绘和导航专业的学子
代码开始
开发的第一部分是:文件读取,文件读取包括两部分,观测文件和星历文件读取。
首先介绍第一部分文件读取:读取星历文件(代码如下)
读取GPS星历(可以是混合星历)
// An highlighted block
function eph=loadxingli(filename)%%读取rinex3的星历文件
tic;
fprintf('Opening RINEX file: %s\n', filename);
finp = fopen(filename, 'rt');
%%%%星历索引值
no = 2:3; year_idx = 5:8; month_idx = 10:11; day_idx = 13:14;
hour_idx = 16:17; min_idx = 19:20; sec_idx = 22:23;
f1 = 5:23; f2 = 24:42; f3 = 43:61; f4 = 62:80;
%%%跳过文件头
while 1
line=fgetl(finp);
if contains(line,'END OF HEADER')
break;
end
end
%%预设数组
sat=1:32;nav=cell(1,32);
eph=[];SEP=zeros(size(sat));
block_init =zeros(42,1);
while ~feof(finp)%%每次循环8行
line=fgetl(finp);
system=line(1);
if system=='G'
prn = str2double(line(no));
idx = find(prn ==sat);
SEP(idx) = SEP(idx) + 1;
tt = zeros(6,1);
tt(1) = round(str2double(line(year_idx)));
tt(2) = round(str2double(line(month_idx)));
tt(3) = round(str2double(line((day_idx))));
tt(4) = str2double(line(hour_idx));
tt(5) = str2double(line(min_idx));
tt(6) = str2double(line(sec_idx));
[GPSWeekNo, GPSSecond, DOY, DOW] = greg2gps(tt');
mTime = datenum(tt');
block=block_init;
block(1:11) = [tt; GPSWeekNo; GPSSecond; DOY; DOW; mTime];
%%%第一行
block(12:14) = [str2double(line(f2));str2double(line(f3));str2double(line(f4))];
%%%第二行
line=fgetl(finp);
block(15:18) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));str2double(line(f4))];
%%%第三行
line=fgetl(finp);
block(19:22) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));(str2double(line(f4)))^2];
%%%% Line 4
line=fgetl(finp);
block(23:26) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));str2double(line(f4))];
%%%% Line 5
line=fgetl(finp);
block(27:30) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));str2double(line(f4))];
%%%% Line 6
line=fgetl(finp);
block(31:34) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));str2double(line(f4))];
%%%% Line 7
line=fgetl(finp);
block(35:38) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));str2double(line(f4))];
%%%% Line 8(最后两个可以直接设置为NaN,如果报错)
line=fgetl(finp);
block(39:42) = [str2double(line(f1));str2double(line(f2));...
str2double(line(f3));str2double(line(f4))];
%%%存到数组中
nav{1,prn}(:,SEP(idx))=block;
end
end
toc;
eph=nav;
%%%%星历读取完成
关于子程序(greg2gps可以私聊我获得)
datenum函数是2018版matlab自带的
读取观测文件(可以是混合文件)
%%读取rinex3的观测文件,并完成定位
%%%观测文件的结构体,不启用
%RNX=struct('obs_body','approx');
fprintf('open obs file; %s\n',filename);
fid=fopen(filename,'rt');
lineidx=0;
P1=6:17;L1=21:33;S1=44:49;
fprintf('open obs file; %s\n',filename);
fid=fopen(filename,'rt');
lineidx=0;
while 1
line=fgetl(fid);
lineidx=lineidx+1;
if contains(line,'APPROX POSITION XYZ')
app_xyz=sscanf(line,'%.4f %.4f %.4f');%计算高度角,随机模型
end
if contains(line,'END OF HEADER')
break;
end
end
%%%读取文件主体
while ~feof(fid)
line=fgetl(fid);
first_str=line(1);
switch first_str
case '>'
lineidx=lineidx+1;
help=0;
sat_num=str2double(line(34:35));
%系数矩阵
xishu=zeros(sat_num,4);
time(1)=str2double(line(3:6));time(2)=str2double(line(8:9));
time(3)=str2double(line(11:12));time(4)=str2double(line(14:15));
time(5)=str2double(line(17:18));time(6)=str2double(line(20:29));
JD = juliandate(datetime(time(1),time(2),time(3)));
gpsweek = floor((JD - 2444244.5)/7);
dow = JD - (2444244.5 + gpsweek*7);
tow = dow*86400 + time(4)*3600 + time(5)*60 + time(6);
doy = JD - juliandate(time(:,1),1,1) + 1;
t_time=[gpsweek, tow, doy, dow];
case 'G'
lineidx=lineidx+1;
sat_id=str2double(line(2:3));
idx=find(sat==sat_id);
sep(idx)=sep(idx)+1;
obs1=str2double(line(P1));
obs2=str2double(line(L1));
obs3=str2double(line(S1));
obs=[t_time(1) t_time(2) obs1 obs2 obs3];
%%%%%%% 这一部分是选取合适的星历,这是是本程序中需要重点改进的部分
eph=nav{1,sat_id}';
eph_idx_ini=7;%需要根据自己的观测文件手动改正
eph_idx= eph_idx_ini+fix(abs(obs(2)-eph(5,8))/7200);
%%%%%%%%(能够完成自动识别,而不是手动改正)
%%%%卫星传播时间矫正
time_s=tow-obs1/299792458;
af2 = eph(eph_idx,14);weekno = eph(eph_idx,33);
af0 = eph(eph_idx,12);toc = eph(eph_idx,8);
af1 = eph(eph_idx,13);timeeph= weekno*7*86400 + toc;
dt = check_t(time_s - timeeph);
corr = (af2 * dt + af1) * dt + af0;
obs(2)=time_s-corr;
%%%%%时间矫正完成
xyz =getSatPosGPS(obs(1:2),eph(eph_idx,:));
%%%%构建法方程,可以用最小二乘也可以使用卡尔曼滤波
%%中间先预留出来,读者自己开发
%%%%
end
end