forked from lqy1989/gps-pseudo-range
-
Notifications
You must be signed in to change notification settings - Fork 0
/
gps.m
70 lines (68 loc) · 4.02 KB
/
gps.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
X0=0;
Y0=0;
Z0=0;
c= 299792458;
%卫星坐标 每行四颗卫星 1 7 8 30
sate=[
-12321.086633 22030.266002 -7898.123088 -59.473993 1568.103921 18329.836568 19208.590888 169.714433 -8308.809901 16236.754272 19295.130320 -105.950399 10508.776501 11521.905826 21507.087625 58.481262
-12968.682280 22445.423463 -5133.514960 -59.477268 97.987077 19817.371725 17677.565974 169.708922 -9229.984552 14159.703118 20491.878139 -105.951456 8833.883151 13308.586146 21211.709476 58.477223
-13405.366899 22635.711957 -2279.220085 -59.480503 -1182.797624 21229.351887 15842.228046 169.703030 -10308.324730 12031.115171 21336.147410 -105.953290 7282.173577 15135.128782 20551.077236 58.473146
-13656.339368 22571.645702 614.978063 -59.483805 -2270.485197 22520.672033 13732.905109 169.697077 -11533.774019 9898.772657 21813.914865 -105.955870 5876.052425 16959.875620 19536.078368 58.469112
-13752.705683 22230.921409 3498.370652 -59.487067 -3169.471793 23647.642265 11384.992441 169.691487 -12888.269053 7808.551113 21917.468387 -105.956993 4630.469559 18738.657742 18183.759770 58.465047
-13730.086628 21599.440097 6320.259515 -59.490346 -3892.012907 24569.682814 8838.410747 169.685616 -14346.329926 5802.817297 21645.504513 -105.957471 3552.490076 20426.388916 16517.064177 58.461025
-13627.041010 20672.014176 9030.905895 -59.493618 -4457.601906 25250.935248 6136.957731 169.679846 -15875.942494 3918.976185 21003.118232 -105.958605 2641.177309 21978.713668 14564.458993 58.457004
-13483.369643 19452.726720 11582.469994 -59.496901 -4892.047239 25661.721684 3327.560721 169.674096 -17439.697544 2188.220321 20001.687455 -105.959464 1887.794977 23353.645948 12359.462131 58.452971
-13338.371338 17954.924858 13929.918982 -59.500144 -5226.280535 25779.787477 459.442017 169.668359 -18996.142635 634.526592 18658.655958 -105.960167 1276.321914 24513.133873 9940.071751 58.448963
-13229.124954 16200.845270 16031.881586 -59.503389 -5494.940879 25591.269976 -2416.787329 169.663099 -20501.294888 -726.064112 16997.219861 -105.961825 784.260138 25424.488191 7348.109116 58.444931
];
sate=sate*1000;
%伪距值 1 7 8 30
pseudo=[
22830483.966 21237513.145 20484722.570 23375458.565
22227585.314 20915096.696 20665048.427 22849492.999
21671442.698 20678571.352 20892760.928 22372344.851
21177599.851 20537686.337 21158851.519 21957426.930
20760311.847 20499004.104 21454929.402 21617382.091
20431681.089 20565188.571 21773573.666 21363246.419
20200838.750 20734667.204 22108591.337 21203592.883
20073223.332 21001635.488 22455109.715 21143731.204
20050211.228 21356502.821 22809626.758 21185165.126
20129055.322 21786590.076 23169925.508 21325338.417
];
for(i=1:10)
for(j=1:10)
R01=sqrt((sate(i,1)-X0)^2+(sate(i,2)-Y0)^2+(sate(i,3)-Z0)^2);
R02=sqrt((sate(i,5)-X0)^2+(sate(i,6)-Y0)^2+(sate(i,7)-Z0)^2);
R03=sqrt((sate(i,9)-X0)^2+(sate(i,10)-Y0)^2+(sate(i,11)-Z0)^2);
R04=sqrt((sate(i,13)-X0)^2+(sate(i,14)-Y0)^2+(sate(i,15)-Z0)^2);
li1=(sate(i,1)-X0)/R01;
li2=(sate(i,5)-X0)/R02;
li3=(sate(i,9)-X0)/R03;
li4=(sate(i,13)-X0)/R04;
li=[li1;li2;li3;li4];
m1=(sate(i,2)-Y0)/R01;
m2=(sate(i,6)-Y0)/R02;
m3=(sate(i,10)-Y0)/R03;
m4=(sate(i,14)-Y0)/R04;
m=[m1;m2;m3;m4];
n1=(sate(i,3)-Z0)/R01;
n2=(sate(i,7)-Z0)/R02;
n3=(sate(i,11)-Z0)/R03;
n4=(sate(i,15)-Z0)/R04;
L1=pseudo(i,1)-R01+c*sate(i,4)*10^-9;
L2=pseudo(i,2)-R02+c*sate(i,8)*10^-9;
L3=pseudo(i,3)-R03+c*sate(i,12)*10^-9;
L4=pseudo(i,4)-R04+c*sate(i,16)*10^-9;
L=[L1;L2;L3;L4];
B=[-li1 -m1 -n1 1;-li2 -m2 -n2 1;-li3 -m3 -n3 1;-li4 -m4 -n4 1];
x=inv(B'*B)*B'*L;
X0=X0+x(1,1);
Y0=Y0+x(2,1);
Z0=Z0+x(3,1);
end
xyz(i,1)=X0;
xyz(i,2)=Y0;
xyz(i,3)=Z0;
end
format long;
xyz