forked from iGNSS/BDS_tdcp
-
Notifications
You must be signed in to change notification settings - Fork 0
/
tdcp_wang.m
112 lines (96 loc) · 2.9 KB
/
tdcp_wang.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
% 可以读取rinex3的多普勒值
clear all
close all
clc
% prn 1-32 GPS; 33-58 GLOMASS; 59-88 GALILEO; 89- BDS
%% TDCP model material
addpath(genpath(pwd));
load options.mat;
fig=1;
% load inf0871.mat;load obs0871.mat;load eph0871.mat;load iono0871.mat
load test_data.mat
% options.system.bds=1;options.system.glo=1;options.system.gal=1;
% files.rinex = 'abpo3350.21o';
% [obs,inf] = read_obsf(files,options);
% [Eph, iono] = RINEX_get_nav('brdm3350.21p');
% compute GPStime(s)
[week,sec_of_week] = gps_time(julday(inf.time.first(1),inf.time.first(2),...
inf.time.first(3),0));
inf.time.week = week;inf.time.sow = sec_of_week;
clear week sec_of_week
for i = 1:length(obs.ep)
obs.epgps(i,1) = obs.ep(i)+ inf.time.sow+inf.time.week*7*24*3600;
end;clear i
% 未考虑高度角
[rPos, sPos, obs,sVel] = spp1(obs,Eph,inf.time.sow);
% obs = cs_detect(obs, inf, options);
figure(fig);plotspp(rPos);fig =fig+1;
% 考虑高度角
%[rPos, ~] = spp2(obs, Eph, inf.time.sow, obs.elv);
rPos = rPos(:,1:size(rPos,2));
figure(fig);plotspp(rPos);fig =fig+1;
figure(fig);plot(rPos(5,:));fig =fig+1;% 绘图PDOP
gamp_pos=load('abpo3350.21o.pos');
rPos_gamp=gamp_pos(:,9:11)';
% obs = cs_detect(obs, inf, options);
delt_x=[];%Qh=cell(1,size(rPos,2));Rh=cell(1,size(rPos,2));
for i=1:size(rPos,2)-1
sats =[];satnum = 0;
for j =94:135 % 1:32 %
if isnan(obs.l1(i,j))==0&&isnan(obs.l1(i+1,j))==0&&obs.elv(i,j)~=0&&obs.elv(i,j)>=10
satnum = satnum+1;
sats(1,satnum) = j;
end
end
% el_i=[];eli=0;
% for a=sats,eli=eli+1;el_i(eli,1)=obs.elv(i,a);el_i(eli,2)=obs.elv(i+1,a);end
% [x, Qhi, Rhi] = tdcp(sats,obs,2,sPos,rPos,i,el_i);
x = tdcp2(sats,obs,sPos,rPos_gamp,i);%,el_i
delt_x = [delt_x x];
end;clear x a j
denu=[];
for i = 1:size(delt_x,2)
delt_x(5,i) = norm(delt_x(1:3,i));
enu = vector_xyz2enu(delt_x(1:3,i),rPos(1:3,i));
denu = [denu enu];
end
%%
yl=0.5;
t=size(delt_x,2);%120;%
figure(fig);fig =fig+1;
subplot(4,1,1)
h = scatter(1:t,delt_x(1,1:t)',1);
ylim([-yl yl]);legend('x')
subplot(4,1,2)
h = scatter(1:t,delt_x(2,1:t)',1);
ylim([-yl yl]);legend('y')
subplot(4,1,3)
h = scatter(1:t,delt_x(3,1:t)',1);
ylim([-yl yl]);legend('z')
subplot(4,1,4)
h = scatter(1:t,delt_x(5,1:t)',1);
ylim([-yl yl]);legend('p')
figure(fig);fig =fig+1;
subplot(4,1,1)
h = scatter(1:t,denu(1,1:t)',1);
ylim([-yl yl]);legend('e')
subplot(4,1,2)
h = scatter(1:t,denu(2,1:t)',1);
ylim([-yl yl]);legend('n')
subplot(4,1,3)
h = scatter(1:t,denu(3,1:t)',1);
ylim([-yl yl]);legend('u')
%%
[HEAD,PITCH] = attitude_from_deltaX(rPos(1:3,:)',delt_x(1:4,:));
[HEAD1,PITCH1] = kalman_smooth(HEAD,PITCH,30,20,30);
% figure(fig);fig=fig+1;
% subplot(2,1,1);plot(HEAD1);
% subplot(2,1,2);plot(PITCH1);
%%
% mean(denu(1,:))
% mean(denu(2,:))
% mean(denu(3,:))
% sqrt(mean(denu(1,:).^2))
% sqrt(mean(denu(2,:).^2))
% sqrt(mean(denu(2,:).^2))
% save eph0871.mat Eph;save obs0871.mat obs;save iono0871.mat iono; save inf0871.mat inf