[2057] | 1 | // Part of BNC, a utility for retrieving decoding and
|
---|
| 2 | // converting GNSS data streams from NTRIP broadcasters.
|
---|
| 3 | //
|
---|
| 4 | // Copyright (C) 2007
|
---|
| 5 | // German Federal Agency for Cartography and Geodesy (BKG)
|
---|
| 6 | // http://www.bkg.bund.de
|
---|
| 7 | // Czech Technical University Prague, Department of Geodesy
|
---|
| 8 | // http://www.fsv.cvut.cz
|
---|
| 9 | //
|
---|
| 10 | // Email: euref-ip@bkg.bund.de
|
---|
| 11 | //
|
---|
| 12 | // This program is free software; you can redistribute it and/or
|
---|
| 13 | // modify it under the terms of the GNU General Public License
|
---|
| 14 | // as published by the Free Software Foundation, version 2.
|
---|
| 15 | //
|
---|
| 16 | // This program is distributed in the hope that it will be useful,
|
---|
| 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
| 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
---|
| 19 | // GNU General Public License for more details.
|
---|
| 20 | //
|
---|
| 21 | // You should have received a copy of the GNU General Public License
|
---|
| 22 | // along with this program; if not, write to the Free Software
|
---|
| 23 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
---|
| 24 |
|
---|
| 25 | /* -------------------------------------------------------------------------
|
---|
| 26 | * BKG NTRIP Client
|
---|
| 27 | * -------------------------------------------------------------------------
|
---|
| 28 | *
|
---|
| 29 | * Class: bncParam, bncModel
|
---|
| 30 | *
|
---|
| 31 | * Purpose: Model for PPP
|
---|
| 32 | *
|
---|
| 33 | * Author: L. Mervart
|
---|
| 34 | *
|
---|
| 35 | * Created: 01-Dec-2009
|
---|
| 36 | *
|
---|
| 37 | * Changes:
|
---|
| 38 | *
|
---|
| 39 | * -----------------------------------------------------------------------*/
|
---|
| 40 |
|
---|
| 41 | #include <iomanip>
|
---|
[2063] | 42 | #include <cmath>
|
---|
[2060] | 43 | #include <newmatio.h>
|
---|
[2057] | 44 |
|
---|
| 45 | #include "bncmodel.h"
|
---|
[2058] | 46 | #include "bncpppclient.h"
|
---|
| 47 | #include "bancroft.h"
|
---|
[2063] | 48 | #include "bncutils.h"
|
---|
[2057] | 49 |
|
---|
| 50 | using namespace std;
|
---|
| 51 |
|
---|
[2070] | 52 | const unsigned MINOBS = 4;
|
---|
| 53 | const double MINELE = 10.0 * M_PI / 180.0;
|
---|
| 54 |
|
---|
[2057] | 55 | // Constructor
|
---|
| 56 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2059] | 57 | bncParam::bncParam(bncParam::parType typeIn) {
|
---|
| 58 | type = typeIn;
|
---|
[2057] | 59 | }
|
---|
| 60 |
|
---|
| 61 | // Destructor
|
---|
| 62 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 63 | bncParam::~bncParam() {
|
---|
| 64 | }
|
---|
| 65 |
|
---|
[2060] | 66 | // Partial
|
---|
| 67 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 68 | double bncParam::partialP3(t_satData* satData) {
|
---|
| 69 | if (type == CRD_X) {
|
---|
| 70 | return (x0 - satData->xx(1)) / satData->rho;
|
---|
| 71 | }
|
---|
| 72 | else if (type == CRD_Y) {
|
---|
| 73 | return (x0 - satData->xx(2)) / satData->rho;
|
---|
| 74 | }
|
---|
| 75 | else if (type == CRD_Z) {
|
---|
| 76 | return (x0 - satData->xx(3)) / satData->rho;
|
---|
| 77 | }
|
---|
| 78 | else if (type == RECCLK) {
|
---|
| 79 | return 1.0;
|
---|
| 80 | }
|
---|
| 81 | return 0.0;
|
---|
| 82 | }
|
---|
| 83 |
|
---|
[2058] | 84 | // Constructor
|
---|
| 85 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 86 | bncModel::bncModel() {
|
---|
[2059] | 87 | _xcBanc.ReSize(4); _xcBanc = 0.0;
|
---|
| 88 | _params.push_back(new bncParam(bncParam::CRD_X));
|
---|
| 89 | _params.push_back(new bncParam(bncParam::CRD_Y));
|
---|
| 90 | _params.push_back(new bncParam(bncParam::CRD_Z));
|
---|
[2060] | 91 | _params.push_back(new bncParam(bncParam::RECCLK));
|
---|
[2064] | 92 | _ellBanc.ReSize(3);
|
---|
[2058] | 93 | }
|
---|
| 94 |
|
---|
| 95 | // Destructor
|
---|
| 96 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 97 | bncModel::~bncModel() {
|
---|
| 98 | }
|
---|
| 99 |
|
---|
| 100 | // Bancroft Solution
|
---|
| 101 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 102 | t_irc bncModel::cmpBancroft(t_epoData* epoData) {
|
---|
| 103 |
|
---|
| 104 | if (epoData->size() < MINOBS) {
|
---|
| 105 | return failure;
|
---|
| 106 | }
|
---|
| 107 |
|
---|
| 108 | Matrix BB(epoData->size(), 4);
|
---|
| 109 |
|
---|
| 110 | QMapIterator<QString, t_satData*> it(epoData->satData);
|
---|
| 111 | int iObs = 0;
|
---|
| 112 | while (it.hasNext()) {
|
---|
[2060] | 113 | ++iObs;
|
---|
[2058] | 114 | it.next();
|
---|
| 115 | QString prn = it.key();
|
---|
| 116 | t_satData* satData = it.value();
|
---|
| 117 | BB(iObs, 1) = satData->xx(1);
|
---|
| 118 | BB(iObs, 2) = satData->xx(2);
|
---|
| 119 | BB(iObs, 3) = satData->xx(3);
|
---|
| 120 | BB(iObs, 4) = satData->P3 + satData->clk;
|
---|
| 121 | }
|
---|
| 122 |
|
---|
| 123 | bancroft(BB, _xcBanc);
|
---|
| 124 |
|
---|
[2060] | 125 | // Set Parameter A Priori Values
|
---|
| 126 | // -----------------------------
|
---|
| 127 | QListIterator<bncParam*> itPar(_params);
|
---|
| 128 | while (itPar.hasNext()) {
|
---|
| 129 | bncParam* par = itPar.next();
|
---|
| 130 | if (par->type == bncParam::CRD_X) {
|
---|
| 131 | par->x0 = _xcBanc(1);
|
---|
| 132 | }
|
---|
| 133 | else if (par->type == bncParam::CRD_Y) {
|
---|
| 134 | par->x0 = _xcBanc(2);
|
---|
| 135 | }
|
---|
| 136 | else if (par->type == bncParam::CRD_Z) {
|
---|
| 137 | par->x0 = _xcBanc(3);
|
---|
| 138 | }
|
---|
| 139 | else if (par->type == bncParam::RECCLK) {
|
---|
| 140 | par->x0 = _xcBanc(4);
|
---|
| 141 | }
|
---|
| 142 | }
|
---|
| 143 |
|
---|
[2064] | 144 | // Ellipsoidal Coordinates
|
---|
| 145 | // ------------------------
|
---|
| 146 | xyz2ell(_xcBanc.data(), _ellBanc.data());
|
---|
[2063] | 147 |
|
---|
[2064] | 148 | // Compute Satellite Elevations
|
---|
| 149 | // ----------------------------
|
---|
| 150 | QMutableMapIterator<QString, t_satData*> it2(epoData->satData);
|
---|
| 151 | while (it2.hasNext()) {
|
---|
| 152 | it2.next();
|
---|
| 153 | QString prn = it2.key();
|
---|
| 154 | t_satData* satData = it2.value();
|
---|
[2063] | 155 |
|
---|
[2064] | 156 | ColumnVector dx = satData->xx - _xcBanc.Rows(1,3);
|
---|
| 157 | double rho = dx.norm_Frobenius();
|
---|
| 158 |
|
---|
| 159 | double neu[3];
|
---|
| 160 | xyz2neu(_ellBanc.data(), dx.data(), neu);
|
---|
| 161 |
|
---|
| 162 | satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
|
---|
| 163 | if (neu[2] < 0) {
|
---|
| 164 | satData->eleSat *= -1.0;
|
---|
| 165 | }
|
---|
| 166 | satData->azSat = atan2(neu[1], neu[0]);
|
---|
[2070] | 167 |
|
---|
| 168 | if (satData->eleSat < MINELE) {
|
---|
| 169 | delete satData;
|
---|
| 170 | it2.remove();
|
---|
| 171 | }
|
---|
[2064] | 172 | }
|
---|
| 173 |
|
---|
[2058] | 174 | return success;
|
---|
| 175 | }
|
---|
[2060] | 176 |
|
---|
| 177 | // Computed Value
|
---|
| 178 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 179 | double bncModel::cmpValueP3(t_satData* satData) {
|
---|
| 180 |
|
---|
| 181 | double rho0 = (satData->xx - _xcBanc.Rows(1,3)).norm_Frobenius();
|
---|
| 182 |
|
---|
| 183 | ColumnVector xRec(3);
|
---|
| 184 | double dPhi = t_CST::omega * rho0 / t_CST::c;
|
---|
| 185 | xRec(1) = _xcBanc(1) * cos(dPhi) - _xcBanc(2) * sin(dPhi);
|
---|
| 186 | xRec(2) = _xcBanc(2) * cos(dPhi) + _xcBanc(1) * sin(dPhi);
|
---|
| 187 | xRec(3) = _xcBanc(3);
|
---|
| 188 |
|
---|
| 189 | satData->rho = (satData->xx - xRec).norm_Frobenius();
|
---|
| 190 |
|
---|
[2065] | 191 | double tropDelay = delay_saast(satData->eleSat);
|
---|
[2060] | 192 |
|
---|
[2061] | 193 | return satData->rho + _xcBanc(4) - satData->clk + tropDelay;
|
---|
[2060] | 194 | }
|
---|
| 195 |
|
---|
[2063] | 196 | // Tropospheric Model (Saastamoinen)
|
---|
| 197 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2065] | 198 | double bncModel::delay_saast(double Ele) {
|
---|
[2063] | 199 |
|
---|
[2064] | 200 | double height = _ellBanc(3);
|
---|
[2063] | 201 |
|
---|
[2064] | 202 | double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
|
---|
| 203 | double TT = 18.0 - height * 0.0065 + 273.15;
|
---|
| 204 | double hh = 50.0 * exp(-6.396e-4 * height);
|
---|
[2063] | 205 | double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
|
---|
| 206 |
|
---|
[2064] | 207 | double h_km = height / 1000.0;
|
---|
[2063] | 208 |
|
---|
| 209 | if (h_km < 0.0) h_km = 0.0;
|
---|
| 210 | if (h_km > 5.0) h_km = 5.0;
|
---|
| 211 | int ii = int(h_km + 1);
|
---|
| 212 | double href = ii - 1;
|
---|
| 213 |
|
---|
| 214 | double bCor[6];
|
---|
| 215 | bCor[0] = 1.156;
|
---|
| 216 | bCor[1] = 1.006;
|
---|
| 217 | bCor[2] = 0.874;
|
---|
| 218 | bCor[3] = 0.757;
|
---|
| 219 | bCor[4] = 0.654;
|
---|
| 220 | bCor[5] = 0.563;
|
---|
| 221 |
|
---|
| 222 | double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
|
---|
| 223 |
|
---|
| 224 | double zen = M_PI/2.0 - Ele;
|
---|
| 225 |
|
---|
| 226 | return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
|
---|
| 227 | }
|
---|
| 228 |
|
---|
[2060] | 229 | // Update Step of the Filter (currently just a single-epoch solution)
|
---|
| 230 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 231 | t_irc bncModel::update(t_epoData* epoData) {
|
---|
| 232 |
|
---|
[2070] | 233 | if (epoData->size() < MINOBS) {
|
---|
| 234 | return failure;
|
---|
| 235 | }
|
---|
| 236 |
|
---|
[2060] | 237 | unsigned nPar = _params.size();
|
---|
| 238 | unsigned nObs = epoData->size();
|
---|
| 239 |
|
---|
[2066] | 240 | // Create First-Design Matrix
|
---|
| 241 | // --------------------------
|
---|
[2060] | 242 | _AA.ReSize(nObs, nPar); // variance-covariance matrix
|
---|
| 243 | _ll.ReSize(nObs); // tems observed-computed
|
---|
| 244 |
|
---|
| 245 | unsigned iObs = 0;
|
---|
| 246 | QMapIterator<QString, t_satData*> itObs(epoData->satData);
|
---|
| 247 | while (itObs.hasNext()) {
|
---|
| 248 | ++iObs;
|
---|
| 249 | itObs.next();
|
---|
| 250 | QString prn = itObs.key();
|
---|
| 251 | t_satData* satData = itObs.value();
|
---|
[2061] | 252 | _ll(iObs) = satData->P3 - cmpValueP3(satData);
|
---|
[2060] | 253 |
|
---|
| 254 | unsigned iPar = 0;
|
---|
| 255 | QListIterator<bncParam*> itPar(_params);
|
---|
| 256 | while (itPar.hasNext()) {
|
---|
| 257 | ++iPar;
|
---|
| 258 | bncParam* par = itPar.next();
|
---|
| 259 | _AA(iObs, iPar) = par->partialP3(satData);
|
---|
| 260 | }
|
---|
| 261 | }
|
---|
| 262 |
|
---|
[2066] | 263 | // Compute Least-Squares Solution
|
---|
| 264 | // ------------------------------
|
---|
[2060] | 265 | _QQ.ReSize(nPar);
|
---|
| 266 | _QQ << _AA.t() * _AA;
|
---|
| 267 | _QQ = _QQ.i();
|
---|
| 268 | _dx = _QQ * _AA.t() * _ll;
|
---|
| 269 |
|
---|
[2066] | 270 | // Compute Residuals
|
---|
| 271 | // -----------------
|
---|
| 272 | ColumnVector vv = _AA * _dx - _ll;
|
---|
| 273 |
|
---|
[2070] | 274 | cout << setprecision(3) << _dx.t()
|
---|
| 275 | << setprecision(3) << vv.t() << endl;
|
---|
| 276 | cout.flush();
|
---|
| 277 |
|
---|
[2066] | 278 | // Set Solution Vector
|
---|
| 279 | // -------------------
|
---|
[2060] | 280 | _xx.ReSize(nPar);
|
---|
| 281 | unsigned iPar = 0;
|
---|
| 282 | QListIterator<bncParam*> itPar(_params);
|
---|
| 283 | while (itPar.hasNext()) {
|
---|
| 284 | ++iPar;
|
---|
| 285 | bncParam* par = itPar.next();
|
---|
| 286 | _xx(iPar) = par->x0 + _dx(iPar);
|
---|
| 287 | }
|
---|
| 288 |
|
---|
| 289 | return success;
|
---|
| 290 | }
|
---|