[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 |
|
---|
| 52 | // Constructor
|
---|
| 53 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2059] | 54 | bncParam::bncParam(bncParam::parType typeIn) {
|
---|
| 55 | type = typeIn;
|
---|
[2057] | 56 | }
|
---|
| 57 |
|
---|
| 58 | // Destructor
|
---|
| 59 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 60 | bncParam::~bncParam() {
|
---|
| 61 | }
|
---|
| 62 |
|
---|
[2060] | 63 | // Partial
|
---|
| 64 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 65 | double bncParam::partialP3(t_satData* satData) {
|
---|
| 66 | if (type == CRD_X) {
|
---|
| 67 | return (x0 - satData->xx(1)) / satData->rho;
|
---|
| 68 | }
|
---|
| 69 | else if (type == CRD_Y) {
|
---|
| 70 | return (x0 - satData->xx(2)) / satData->rho;
|
---|
| 71 | }
|
---|
| 72 | else if (type == CRD_Z) {
|
---|
| 73 | return (x0 - satData->xx(3)) / satData->rho;
|
---|
| 74 | }
|
---|
| 75 | else if (type == RECCLK) {
|
---|
| 76 | return 1.0;
|
---|
| 77 | }
|
---|
| 78 | return 0.0;
|
---|
| 79 | }
|
---|
| 80 |
|
---|
[2058] | 81 | // Constructor
|
---|
| 82 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 83 | bncModel::bncModel() {
|
---|
[2059] | 84 | _xcBanc.ReSize(4); _xcBanc = 0.0;
|
---|
| 85 | _params.push_back(new bncParam(bncParam::CRD_X));
|
---|
| 86 | _params.push_back(new bncParam(bncParam::CRD_Y));
|
---|
| 87 | _params.push_back(new bncParam(bncParam::CRD_Z));
|
---|
[2060] | 88 | _params.push_back(new bncParam(bncParam::RECCLK));
|
---|
[2064] | 89 | _ellBanc.ReSize(3);
|
---|
[2058] | 90 | }
|
---|
| 91 |
|
---|
| 92 | // Destructor
|
---|
| 93 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 94 | bncModel::~bncModel() {
|
---|
| 95 | }
|
---|
| 96 |
|
---|
| 97 | // Bancroft Solution
|
---|
| 98 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 99 | t_irc bncModel::cmpBancroft(t_epoData* epoData) {
|
---|
| 100 |
|
---|
| 101 | const unsigned MINOBS = 4;
|
---|
| 102 |
|
---|
| 103 | if (epoData->size() < MINOBS) {
|
---|
| 104 | return failure;
|
---|
| 105 | }
|
---|
| 106 |
|
---|
| 107 | Matrix BB(epoData->size(), 4);
|
---|
| 108 |
|
---|
| 109 | QMapIterator<QString, t_satData*> it(epoData->satData);
|
---|
| 110 | int iObs = 0;
|
---|
| 111 | while (it.hasNext()) {
|
---|
[2060] | 112 | ++iObs;
|
---|
[2058] | 113 | it.next();
|
---|
| 114 | QString prn = it.key();
|
---|
| 115 | t_satData* satData = it.value();
|
---|
| 116 | BB(iObs, 1) = satData->xx(1);
|
---|
| 117 | BB(iObs, 2) = satData->xx(2);
|
---|
| 118 | BB(iObs, 3) = satData->xx(3);
|
---|
| 119 | BB(iObs, 4) = satData->P3 + satData->clk;
|
---|
| 120 | }
|
---|
| 121 |
|
---|
| 122 | bancroft(BB, _xcBanc);
|
---|
| 123 |
|
---|
[2060] | 124 | // Set Parameter A Priori Values
|
---|
| 125 | // -----------------------------
|
---|
| 126 | QListIterator<bncParam*> itPar(_params);
|
---|
| 127 | while (itPar.hasNext()) {
|
---|
| 128 | bncParam* par = itPar.next();
|
---|
| 129 | if (par->type == bncParam::CRD_X) {
|
---|
| 130 | par->x0 = _xcBanc(1);
|
---|
| 131 | }
|
---|
| 132 | else if (par->type == bncParam::CRD_Y) {
|
---|
| 133 | par->x0 = _xcBanc(2);
|
---|
| 134 | }
|
---|
| 135 | else if (par->type == bncParam::CRD_Z) {
|
---|
| 136 | par->x0 = _xcBanc(3);
|
---|
| 137 | }
|
---|
| 138 | else if (par->type == bncParam::RECCLK) {
|
---|
| 139 | par->x0 = _xcBanc(4);
|
---|
| 140 | }
|
---|
| 141 | }
|
---|
| 142 |
|
---|
[2064] | 143 | // Ellipsoidal Coordinates
|
---|
| 144 | // ------------------------
|
---|
| 145 | xyz2ell(_xcBanc.data(), _ellBanc.data());
|
---|
[2063] | 146 |
|
---|
[2064] | 147 | // Compute Satellite Elevations
|
---|
| 148 | // ----------------------------
|
---|
| 149 | QMutableMapIterator<QString, t_satData*> it2(epoData->satData);
|
---|
| 150 | while (it2.hasNext()) {
|
---|
| 151 | it2.next();
|
---|
| 152 | QString prn = it2.key();
|
---|
| 153 | t_satData* satData = it2.value();
|
---|
[2063] | 154 |
|
---|
[2064] | 155 | ColumnVector dx = satData->xx - _xcBanc.Rows(1,3);
|
---|
| 156 | double rho = dx.norm_Frobenius();
|
---|
| 157 |
|
---|
| 158 | double neu[3];
|
---|
| 159 | xyz2neu(_ellBanc.data(), dx.data(), neu);
|
---|
| 160 |
|
---|
| 161 | satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
|
---|
| 162 | if (neu[2] < 0) {
|
---|
| 163 | satData->eleSat *= -1.0;
|
---|
| 164 | }
|
---|
| 165 | satData->azSat = atan2(neu[1], neu[0]);
|
---|
| 166 | }
|
---|
| 167 |
|
---|
[2058] | 168 | return success;
|
---|
| 169 | }
|
---|
[2060] | 170 |
|
---|
| 171 | // Computed Value
|
---|
| 172 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 173 | double bncModel::cmpValueP3(t_satData* satData) {
|
---|
| 174 |
|
---|
| 175 | double rho0 = (satData->xx - _xcBanc.Rows(1,3)).norm_Frobenius();
|
---|
| 176 |
|
---|
| 177 | ColumnVector xRec(3);
|
---|
| 178 | double dPhi = t_CST::omega * rho0 / t_CST::c;
|
---|
| 179 | xRec(1) = _xcBanc(1) * cos(dPhi) - _xcBanc(2) * sin(dPhi);
|
---|
| 180 | xRec(2) = _xcBanc(2) * cos(dPhi) + _xcBanc(1) * sin(dPhi);
|
---|
| 181 | xRec(3) = _xcBanc(3);
|
---|
| 182 |
|
---|
| 183 | satData->rho = (satData->xx - xRec).norm_Frobenius();
|
---|
| 184 |
|
---|
[2065] | 185 | double tropDelay = delay_saast(satData->eleSat);
|
---|
[2060] | 186 |
|
---|
[2063] | 187 | cout << "tropDelay " << tropDelay << endl;
|
---|
| 188 |
|
---|
[2061] | 189 | return satData->rho + _xcBanc(4) - satData->clk + tropDelay;
|
---|
[2060] | 190 | }
|
---|
| 191 |
|
---|
[2063] | 192 | // Tropospheric Model (Saastamoinen)
|
---|
| 193 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2065] | 194 | double bncModel::delay_saast(double Ele) {
|
---|
[2063] | 195 |
|
---|
[2064] | 196 | double height = _ellBanc(3);
|
---|
[2063] | 197 |
|
---|
[2064] | 198 | double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
|
---|
| 199 | double TT = 18.0 - height * 0.0065 + 273.15;
|
---|
| 200 | double hh = 50.0 * exp(-6.396e-4 * height);
|
---|
[2063] | 201 | double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
|
---|
| 202 |
|
---|
[2064] | 203 | double h_km = height / 1000.0;
|
---|
[2063] | 204 |
|
---|
| 205 | if (h_km < 0.0) h_km = 0.0;
|
---|
| 206 | if (h_km > 5.0) h_km = 5.0;
|
---|
| 207 | int ii = int(h_km + 1);
|
---|
| 208 | double href = ii - 1;
|
---|
| 209 |
|
---|
| 210 | double bCor[6];
|
---|
| 211 | bCor[0] = 1.156;
|
---|
| 212 | bCor[1] = 1.006;
|
---|
| 213 | bCor[2] = 0.874;
|
---|
| 214 | bCor[3] = 0.757;
|
---|
| 215 | bCor[4] = 0.654;
|
---|
| 216 | bCor[5] = 0.563;
|
---|
| 217 |
|
---|
| 218 | double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
|
---|
| 219 |
|
---|
| 220 | double zen = M_PI/2.0 - Ele;
|
---|
| 221 |
|
---|
| 222 | return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
|
---|
| 223 | }
|
---|
| 224 |
|
---|
[2060] | 225 | // Update Step of the Filter (currently just a single-epoch solution)
|
---|
| 226 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 227 | t_irc bncModel::update(t_epoData* epoData) {
|
---|
| 228 |
|
---|
| 229 | unsigned nPar = _params.size();
|
---|
| 230 | unsigned nObs = epoData->size();
|
---|
| 231 |
|
---|
| 232 | _AA.ReSize(nObs, nPar); // variance-covariance matrix
|
---|
| 233 | _ll.ReSize(nObs); // tems observed-computed
|
---|
| 234 |
|
---|
| 235 | unsigned iObs = 0;
|
---|
| 236 | QMapIterator<QString, t_satData*> itObs(epoData->satData);
|
---|
| 237 | while (itObs.hasNext()) {
|
---|
| 238 | ++iObs;
|
---|
| 239 | itObs.next();
|
---|
| 240 | QString prn = itObs.key();
|
---|
| 241 | t_satData* satData = itObs.value();
|
---|
[2061] | 242 | _ll(iObs) = satData->P3 - cmpValueP3(satData);
|
---|
[2060] | 243 |
|
---|
| 244 | unsigned iPar = 0;
|
---|
| 245 | QListIterator<bncParam*> itPar(_params);
|
---|
| 246 | while (itPar.hasNext()) {
|
---|
| 247 | ++iPar;
|
---|
| 248 | bncParam* par = itPar.next();
|
---|
| 249 | _AA(iObs, iPar) = par->partialP3(satData);
|
---|
| 250 | }
|
---|
| 251 | }
|
---|
| 252 |
|
---|
| 253 | _QQ.ReSize(nPar);
|
---|
| 254 | _QQ << _AA.t() * _AA;
|
---|
| 255 | _QQ = _QQ.i();
|
---|
| 256 | _dx = _QQ * _AA.t() * _ll;
|
---|
| 257 |
|
---|
| 258 | _xx.ReSize(nPar);
|
---|
| 259 |
|
---|
| 260 | unsigned iPar = 0;
|
---|
| 261 | QListIterator<bncParam*> itPar(_params);
|
---|
| 262 | while (itPar.hasNext()) {
|
---|
| 263 | ++iPar;
|
---|
| 264 | bncParam* par = itPar.next();
|
---|
| 265 | _xx(iPar) = par->x0 + _dx(iPar);
|
---|
| 266 | }
|
---|
| 267 |
|
---|
| 268 | return success;
|
---|
| 269 | }
|
---|