Added some gpstk based application, to better understand gpstk
[gps] / gpstk-solution2.cpp
1 #include <iostream>
2 #include "SP3EphemerisStore.hpp"
3 #include "Position.hpp"
4
5 #include "DataStructures.hpp"
6 #include "SimpleFilter.hpp"
7 #include "IonoModel.hpp"
8 #include "IonoModelStore.hpp"
9 #include "ModelObs.hpp"
10 #include "SolverLMS.hpp"
11 #include "SolverPPP.hpp"
12
13 using namespace std;
14 using namespace gpstk;
15
16 int main() {
17
18         DayTime dayTime(1667, 66448.850000);
19
20         /* satellites list */
21         SatID sat2 ( 2, SatID::systemGPS);
22         SatID sat4 ( 4, SatID::systemGPS);
23         SatID sat5 ( 5, SatID::systemGPS);
24         SatID sat7 ( 7, SatID::systemGPS);
25         SatID sat10(10, SatID::systemGPS);
26         SatID sat13(13, SatID::systemGPS);
27         SatID sat29(29, SatID::systemGPS);
28         
29         /*gnssRinex*/ gnssSatTypeValue gRin;
30         /* header = sourceEpochTypeHeader + inheritance from gnssSatTypeValue
31            = gnssData<sourceEpochHeader, satTypeValueMap
32            satTypeValueMap = std::map<SatID, typeValueMap> 
33            typeValueMap == std::map<TypeID, double>
34         */
35
36         gRin.header.epoch = dayTime;
37
38         typeValueMap TVmap;
39         satTypeValueMap STVmap;
40
41         STVmap.clear();
42
43         TVmap.clear();
44         TVmap[TypeID::C1] = 21375681.860758;
45         STVmap[sat2 ] = TVmap;
46
47         TVmap.clear();
48         TVmap[TypeID::C1] = 21808239.424646;
49         STVmap[sat4 ] = TVmap;
50
51         TVmap.clear();
52         TVmap[TypeID::C1] = 23834064.766712;
53         STVmap[sat5 ] = TVmap;
54
55         TVmap.clear();
56         TVmap[TypeID::C1] = 21213754.205750;
57         STVmap[sat7 ] = TVmap;
58
59         TVmap.clear();
60         TVmap[TypeID::C1] = 20192220.627359;
61         STVmap[sat10] = TVmap;
62
63         TVmap.clear();
64         TVmap[TypeID::C1] = 20794005.449315;
65         STVmap[sat13] = TVmap;
66
67         TVmap.clear();
68         TVmap[TypeID::C1] = 25306963.567147;
69         STVmap[sat29] = TVmap;
70
71         gRin.body = STVmap;
72
73         SP3EphemerisStore ephemerisStore;
74         // from
75         // ftp://rgpdata.ign.fr/pub/products/ephemerides/1667/esr16670.sp3.Z
76         ephemerisStore.loadFile("esr16670.sp3");
77
78         /* we got (the results depends on RMSLimit) : 
79                    x = 4205773.319
80                    y =  166187.065
81                    z = 4776312.107
82
83                    SiRF Star III was computing (sat 2, 10 & 4) :
84                      X-position : 4205766 m
85                      Y-position :  166220 m
86                      Z-position : 4776300 m
87         */
88
89                 /* current position is (as reported by gpsd) : 
90                    longitude =   2.262773
91                    latitude  =  48.803081
92                    altitude  = 156.445000
93                 */
94
95         try {
96                 SimpleFilter filter;
97
98                 /* FIXME : what is the role of nominalPos ? */
99                 Position nominalPos(4205000.0, 166000.0, 4776000.0);
100
101                 double ionAlpha[4] = { 0.0 , 0.0, 0.0, 0.0 };
102                 double ionBeta[4]  = { 149500.0, -131100.0, 0.0, -131100.0 };
103
104                 IonoModel ioModel;
105                 ioModel.setModel(ionAlpha, ionBeta);
106
107                 IonoModelStore ionoStore;
108                 ionoStore.addIonoModel(DayTime::BEGINNING_OF_TIME, ioModel);
109
110                 MOPSTropModel mopsTM( nominalPos.getAltitude(),
111                                       nominalPos.getGeodeticLatitude(),
112                                       30 );
113
114
115                 ModelObs model(/* IonoModelStore */ ionoStore,
116                                /* TropModel */ mopsTM, 
117                                /* XvtStore<SatID> */ ephemerisStore,
118                                /* TypeID */ TypeID::C1);
119                 model.setModelPrepared(false);
120
121                 SolverLMS solver;
122
123                 cout << fixed << setprecision(6);
124
125                 //model.Prepare(gRin.header.epoch, gRin.body);
126
127                 cout << "model.getModelPrepared = " 
128                      << model.getModelPrepared() << endl;
129
130                 //cout << "GDS data at start : " << endl;
131                 //gRin.body.dump(cout, 1);
132                 gRin >> filter >> model >> solver;
133                 //cout << "GDS data after 'filter+model+solver' : " <<endl;
134                 //gRin.body.dump(cout,1);
135
136                 cout << "Solution is : " << solver.isValid() << endl;
137
138                 cout << "dx = " << solver.getSolution(TypeID::dx) << endl;
139                 cout << "dy = " << solver.getSolution(TypeID::dy) << endl;
140                 cout << "dz = " << solver.getSolution(TypeID::dz) << endl;
141                 cout << "cdt = " << solver.getSolution(TypeID::cdt) << endl;
142
143                 cout << "model.rxPos is : " <<endl;
144                 cout << "x = " << model.rxPos.X() << endl;
145                 cout << "y = " << model.rxPos.Y() << endl;
146                 cout << "z = " << model.rxPos.Z() << endl;
147
148                 /*
149                   model.rxPos is : 
150                   x = 4205773.614814
151                   y = 166221.288957
152                   z = 4776329.263813
153                 */
154
155                 cout << "ECEF result : " << endl;
156                 cout << "x = " << model.rxPos.X()+solver.getSolution(TypeID::dx)
157                      << endl;
158                 cout << "y = " << model.rxPos.Y()+solver.getSolution(TypeID::dy)
159                      << endl;
160                 cout << "z = " << model.rxPos.Z()+solver.getSolution(TypeID::dz)
161                      << endl;
162
163                 /*
164                   ECEF result : 
165                   x = 4205741.185280
166                   y = 166208.570230
167                   z = 4776301.430305
168                  */
169
170                 Position pos(model.rxPos.X()+solver.getSolution(TypeID::dx),
171                              model.rxPos.Y()+solver.getSolution(TypeID::dy),
172                              model.rxPos.Z()+solver.getSolution(TypeID::dz),
173                              Position::Cartesian);
174                 cout <<"longitude = " << pos.asGeodetic().getLongitude()<<endl;
175                 cout <<"latitude = "<<pos.asGeodetic().geodeticLatitude()<<endl;
176                 cout <<"altitude = " << pos.asGeodetic().getAltitude() << endl;
177
178                 /*
179                   longitude = 2.263120
180                   latitude = 48.803313
181                   altitude = 160.905461
182                 */
183         }
184
185         catch (...) {
186                 cout << "An exception occurs" << endl;
187         }
188
189         return 0;
190 }