[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"
|
---|
[2077] | 49 | #include "bncsettings.h"
|
---|
[2057] | 50 |
|
---|
| 51 | using namespace std;
|
---|
| 52 |
|
---|
[2070] | 53 | const unsigned MINOBS = 4;
|
---|
| 54 | const double MINELE = 10.0 * M_PI / 180.0;
|
---|
[2073] | 55 | const double sig_crd_0 = 100.0;
|
---|
| 56 | const double sig_crd_p = 100.0;
|
---|
| 57 | const double sig_clk_0 = 1000.0;
|
---|
[2084] | 58 | const double sig_trp_0 = 0.01;
|
---|
| 59 | const double sig_trp_p = 1e-6;
|
---|
[2080] | 60 | const double sig_amb_0 = 100.0;
|
---|
[2081] | 61 | const double sig_P3 = 1.0;
|
---|
[2084] | 62 | const double sig_L3 = 0.01;
|
---|
[2070] | 63 |
|
---|
[2057] | 64 | // Constructor
|
---|
| 65 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2080] | 66 | bncParam::bncParam(bncParam::parType typeIn, int indexIn,
|
---|
| 67 | const QString& prnIn) {
|
---|
| 68 | type = typeIn;
|
---|
| 69 | index = indexIn;
|
---|
| 70 | prn = prnIn;
|
---|
| 71 | index_old = 0;
|
---|
| 72 | xx = 0.0;
|
---|
[2057] | 73 | }
|
---|
| 74 |
|
---|
| 75 | // Destructor
|
---|
| 76 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 77 | bncParam::~bncParam() {
|
---|
| 78 | }
|
---|
| 79 |
|
---|
[2060] | 80 | // Partial
|
---|
| 81 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2080] | 82 | double bncParam::partial(t_satData* satData, const QString& prnIn) {
|
---|
[2060] | 83 | if (type == CRD_X) {
|
---|
[2109] | 84 | return (xx - satData->xx(1)) / satData->rho;
|
---|
[2060] | 85 | }
|
---|
| 86 | else if (type == CRD_Y) {
|
---|
[2109] | 87 | return (xx - satData->xx(2)) / satData->rho;
|
---|
[2060] | 88 | }
|
---|
| 89 | else if (type == CRD_Z) {
|
---|
[2109] | 90 | return (xx - satData->xx(3)) / satData->rho;
|
---|
[2060] | 91 | }
|
---|
| 92 | else if (type == RECCLK) {
|
---|
| 93 | return 1.0;
|
---|
| 94 | }
|
---|
[2084] | 95 | else if (type == TROPO) {
|
---|
| 96 | return 1.0 / sin(satData->eleSat);
|
---|
| 97 | }
|
---|
[2080] | 98 | else if (type == AMB_L3) {
|
---|
| 99 | if (prnIn == prn) {
|
---|
| 100 | return 1.0;
|
---|
| 101 | }
|
---|
| 102 | else {
|
---|
| 103 | return 0.0;
|
---|
| 104 | }
|
---|
| 105 | }
|
---|
[2060] | 106 | return 0.0;
|
---|
| 107 | }
|
---|
| 108 |
|
---|
[2058] | 109 | // Constructor
|
---|
| 110 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 111 | bncModel::bncModel() {
|
---|
[2084] | 112 |
|
---|
| 113 | bncSettings settings;
|
---|
| 114 |
|
---|
| 115 | _static = false;
|
---|
| 116 | if ( Qt::CheckState(settings.value("pppStatic").toInt()) == Qt::Checked) {
|
---|
| 117 | _static = true;
|
---|
| 118 | }
|
---|
| 119 |
|
---|
| 120 | _usePhase = false;
|
---|
| 121 | if ( Qt::CheckState(settings.value("pppUsePhase").toInt()) == Qt::Checked) {
|
---|
| 122 | _usePhase = true;
|
---|
| 123 | }
|
---|
| 124 |
|
---|
| 125 | _estTropo = false;
|
---|
| 126 | if ( Qt::CheckState(settings.value("pppEstTropo").toInt()) == Qt::Checked) {
|
---|
| 127 | _estTropo = true;
|
---|
| 128 | }
|
---|
| 129 |
|
---|
| 130 | _xcBanc.ReSize(4); _xcBanc = 0.0;
|
---|
| 131 | _ellBanc.ReSize(3); _ellBanc = 0.0;
|
---|
| 132 |
|
---|
[2080] | 133 | _params.push_back(new bncParam(bncParam::CRD_X, 1, ""));
|
---|
| 134 | _params.push_back(new bncParam(bncParam::CRD_Y, 2, ""));
|
---|
| 135 | _params.push_back(new bncParam(bncParam::CRD_Z, 3, ""));
|
---|
| 136 | _params.push_back(new bncParam(bncParam::RECCLK, 4, ""));
|
---|
[2084] | 137 | if (_estTropo) {
|
---|
| 138 | _params.push_back(new bncParam(bncParam::TROPO, 5, ""));
|
---|
| 139 | }
|
---|
[2073] | 140 |
|
---|
| 141 | unsigned nPar = _params.size();
|
---|
[2084] | 142 |
|
---|
[2073] | 143 | _QQ.ReSize(nPar);
|
---|
| 144 | _QQ = 0.0;
|
---|
| 145 |
|
---|
| 146 | _QQ(1,1) = sig_crd_0 * sig_crd_0;
|
---|
| 147 | _QQ(2,2) = sig_crd_0 * sig_crd_0;
|
---|
| 148 | _QQ(3,3) = sig_crd_0 * sig_crd_0;
|
---|
| 149 | _QQ(4,4) = sig_clk_0 * sig_clk_0;
|
---|
[2084] | 150 | if (_estTropo) {
|
---|
| 151 | _QQ(5,5) = sig_trp_0 * sig_trp_0;
|
---|
[2077] | 152 | }
|
---|
[2058] | 153 | }
|
---|
| 154 |
|
---|
| 155 | // Destructor
|
---|
| 156 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 157 | bncModel::~bncModel() {
|
---|
| 158 | }
|
---|
| 159 |
|
---|
| 160 | // Bancroft Solution
|
---|
| 161 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 162 | t_irc bncModel::cmpBancroft(t_epoData* epoData) {
|
---|
| 163 |
|
---|
| 164 | if (epoData->size() < MINOBS) {
|
---|
| 165 | return failure;
|
---|
| 166 | }
|
---|
| 167 |
|
---|
| 168 | Matrix BB(epoData->size(), 4);
|
---|
| 169 |
|
---|
| 170 | QMapIterator<QString, t_satData*> it(epoData->satData);
|
---|
| 171 | int iObs = 0;
|
---|
| 172 | while (it.hasNext()) {
|
---|
[2060] | 173 | ++iObs;
|
---|
[2058] | 174 | it.next();
|
---|
| 175 | QString prn = it.key();
|
---|
| 176 | t_satData* satData = it.value();
|
---|
| 177 | BB(iObs, 1) = satData->xx(1);
|
---|
| 178 | BB(iObs, 2) = satData->xx(2);
|
---|
| 179 | BB(iObs, 3) = satData->xx(3);
|
---|
| 180 | BB(iObs, 4) = satData->P3 + satData->clk;
|
---|
| 181 | }
|
---|
| 182 |
|
---|
| 183 | bancroft(BB, _xcBanc);
|
---|
| 184 |
|
---|
[2064] | 185 | // Ellipsoidal Coordinates
|
---|
| 186 | // ------------------------
|
---|
| 187 | xyz2ell(_xcBanc.data(), _ellBanc.data());
|
---|
[2063] | 188 |
|
---|
[2064] | 189 | // Compute Satellite Elevations
|
---|
| 190 | // ----------------------------
|
---|
| 191 | QMutableMapIterator<QString, t_satData*> it2(epoData->satData);
|
---|
| 192 | while (it2.hasNext()) {
|
---|
| 193 | it2.next();
|
---|
| 194 | QString prn = it2.key();
|
---|
| 195 | t_satData* satData = it2.value();
|
---|
[2063] | 196 |
|
---|
[2109] | 197 | ColumnVector rr = satData->xx - _xcBanc.Rows(1,3);
|
---|
| 198 | double rho = rr.norm_Frobenius();
|
---|
[2064] | 199 |
|
---|
| 200 | double neu[3];
|
---|
[2109] | 201 | xyz2neu(_ellBanc.data(), rr.data(), neu);
|
---|
[2064] | 202 |
|
---|
| 203 | satData->eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
|
---|
| 204 | if (neu[2] < 0) {
|
---|
| 205 | satData->eleSat *= -1.0;
|
---|
| 206 | }
|
---|
| 207 | satData->azSat = atan2(neu[1], neu[0]);
|
---|
[2070] | 208 |
|
---|
| 209 | if (satData->eleSat < MINELE) {
|
---|
| 210 | delete satData;
|
---|
| 211 | it2.remove();
|
---|
| 212 | }
|
---|
[2064] | 213 | }
|
---|
| 214 |
|
---|
[2058] | 215 | return success;
|
---|
| 216 | }
|
---|
[2060] | 217 |
|
---|
| 218 | // Computed Value
|
---|
| 219 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2080] | 220 | double bncModel::cmpValue(t_satData* satData) {
|
---|
[2060] | 221 |
|
---|
[2073] | 222 | ColumnVector xRec(3);
|
---|
| 223 | xRec(1) = x();
|
---|
| 224 | xRec(2) = y();
|
---|
| 225 | xRec(3) = z();
|
---|
[2060] | 226 |
|
---|
[2073] | 227 | double rho0 = (satData->xx - xRec).norm_Frobenius();
|
---|
[2060] | 228 | double dPhi = t_CST::omega * rho0 / t_CST::c;
|
---|
| 229 |
|
---|
[2073] | 230 | xRec(1) = x() * cos(dPhi) - y() * sin(dPhi);
|
---|
| 231 | xRec(2) = y() * cos(dPhi) + x() * sin(dPhi);
|
---|
| 232 | xRec(3) = z();
|
---|
| 233 |
|
---|
[2060] | 234 | satData->rho = (satData->xx - xRec).norm_Frobenius();
|
---|
| 235 |
|
---|
[2084] | 236 | double tropDelay = delay_saast(satData->eleSat) +
|
---|
| 237 | trp() / sin(satData->eleSat);
|
---|
[2060] | 238 |
|
---|
[2073] | 239 | return satData->rho + clk() - satData->clk + tropDelay;
|
---|
[2060] | 240 | }
|
---|
| 241 |
|
---|
[2063] | 242 | // Tropospheric Model (Saastamoinen)
|
---|
| 243 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2065] | 244 | double bncModel::delay_saast(double Ele) {
|
---|
[2063] | 245 |
|
---|
[2064] | 246 | double height = _ellBanc(3);
|
---|
[2063] | 247 |
|
---|
[2064] | 248 | double pp = 1013.25 * pow(1.0 - 2.26e-5 * height, 5.225);
|
---|
| 249 | double TT = 18.0 - height * 0.0065 + 273.15;
|
---|
| 250 | double hh = 50.0 * exp(-6.396e-4 * height);
|
---|
[2063] | 251 | double ee = hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
|
---|
| 252 |
|
---|
[2064] | 253 | double h_km = height / 1000.0;
|
---|
[2063] | 254 |
|
---|
| 255 | if (h_km < 0.0) h_km = 0.0;
|
---|
| 256 | if (h_km > 5.0) h_km = 5.0;
|
---|
| 257 | int ii = int(h_km + 1);
|
---|
| 258 | double href = ii - 1;
|
---|
| 259 |
|
---|
| 260 | double bCor[6];
|
---|
| 261 | bCor[0] = 1.156;
|
---|
| 262 | bCor[1] = 1.006;
|
---|
| 263 | bCor[2] = 0.874;
|
---|
| 264 | bCor[3] = 0.757;
|
---|
| 265 | bCor[4] = 0.654;
|
---|
| 266 | bCor[5] = 0.563;
|
---|
| 267 |
|
---|
| 268 | double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
|
---|
| 269 |
|
---|
| 270 | double zen = M_PI/2.0 - Ele;
|
---|
| 271 |
|
---|
| 272 | return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
|
---|
| 273 | }
|
---|
| 274 |
|
---|
[2073] | 275 | // Prediction Step of the Filter
|
---|
| 276 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2080] | 277 | void bncModel::predict(t_epoData* epoData) {
|
---|
[2073] | 278 |
|
---|
[2083] | 279 | if (_usePhase) {
|
---|
[2080] | 280 |
|
---|
[2083] | 281 | // Make a copy of QQ and xx, set parameter indices
|
---|
| 282 | // -----------------------------------------------
|
---|
| 283 | SymmetricMatrix QQ_old = _QQ;
|
---|
| 284 |
|
---|
| 285 | for (int iPar = 1; iPar <= _params.size(); iPar++) {
|
---|
| 286 | _params[iPar-1]->index_old = _params[iPar-1]->index;
|
---|
| 287 | _params[iPar-1]->index = 0;
|
---|
| 288 | }
|
---|
| 289 |
|
---|
| 290 | // Remove Ambiguity Parameters without observations
|
---|
| 291 | // ------------------------------------------------
|
---|
| 292 | int iPar = 0;
|
---|
| 293 | QMutableVectorIterator<bncParam*> it(_params);
|
---|
| 294 | while (it.hasNext()) {
|
---|
| 295 | bncParam* par = it.next();
|
---|
| 296 | bool removed = false;
|
---|
| 297 | if (par->type == bncParam::AMB_L3) {
|
---|
| 298 | if (epoData->satData.find(par->prn) == epoData->satData.end()) {
|
---|
| 299 | removed = true;
|
---|
| 300 | delete par;
|
---|
| 301 | it.remove();
|
---|
| 302 | }
|
---|
[2080] | 303 | }
|
---|
[2083] | 304 | if (! removed) {
|
---|
| 305 | ++iPar;
|
---|
| 306 | par->index = iPar;
|
---|
| 307 | }
|
---|
[2080] | 308 | }
|
---|
[2083] | 309 |
|
---|
| 310 | // Add new ambiguity parameters
|
---|
| 311 | // ----------------------------
|
---|
| 312 | QMapIterator<QString, t_satData*> itObs(epoData->satData);
|
---|
| 313 | while (itObs.hasNext()) {
|
---|
| 314 | itObs.next();
|
---|
| 315 | QString prn = itObs.key();
|
---|
| 316 | bool found = false;
|
---|
| 317 | for (int iPar = 1; iPar <= _params.size(); iPar++) {
|
---|
| 318 | if (_params[iPar-1]->type == bncParam::AMB_L3 &&
|
---|
| 319 | _params[iPar-1]->prn == prn) {
|
---|
| 320 | found = true;
|
---|
| 321 | break;
|
---|
| 322 | }
|
---|
[2080] | 323 | }
|
---|
[2083] | 324 | if (!found) {
|
---|
| 325 | bncParam* par = new bncParam(bncParam::AMB_L3, _params.size()+1, prn);
|
---|
| 326 | _params.push_back(par);
|
---|
| 327 | }
|
---|
[2080] | 328 | }
|
---|
[2083] | 329 |
|
---|
| 330 | int nPar = _params.size();
|
---|
| 331 | _QQ.ReSize(nPar); _QQ = 0.0;
|
---|
| 332 | for (int i1 = 1; i1 <= nPar; i1++) {
|
---|
| 333 | bncParam* p1 = _params[i1-1];
|
---|
| 334 | if (p1->index_old != 0) {
|
---|
| 335 | _QQ(p1->index, p1->index) = QQ_old(p1->index_old, p1->index_old);
|
---|
| 336 | for (int i2 = 1; i2 <= nPar; i2++) {
|
---|
| 337 | bncParam* p2 = _params[i2-1];
|
---|
| 338 | if (p2->index_old != 0) {
|
---|
| 339 | _QQ(p1->index, p2->index) = QQ_old(p1->index_old, p2->index_old);
|
---|
| 340 | }
|
---|
[2080] | 341 | }
|
---|
| 342 | }
|
---|
| 343 | }
|
---|
[2083] | 344 |
|
---|
| 345 | for (int ii = 1; ii <= nPar; ii++) {
|
---|
| 346 | bncParam* par = _params[ii-1];
|
---|
| 347 | if (par->index_old == 0) {
|
---|
| 348 | _QQ(par->index, par->index) = sig_amb_0 * sig_amb_0;
|
---|
| 349 | }
|
---|
| 350 | par->index_old = par->index;
|
---|
[2080] | 351 | }
|
---|
| 352 | }
|
---|
| 353 |
|
---|
[2076] | 354 | // Coordinates
|
---|
| 355 | // -----------
|
---|
| 356 | if (_static) {
|
---|
| 357 | if (x() == 0.0 && y() == 0.0 && z() == 0.0) {
|
---|
[2109] | 358 | _params[0]->xx = _xcBanc(1);
|
---|
| 359 | _params[1]->xx = _xcBanc(2);
|
---|
| 360 | _params[2]->xx = _xcBanc(3);
|
---|
[2076] | 361 | }
|
---|
| 362 | }
|
---|
| 363 | else {
|
---|
[2109] | 364 | _params[0]->xx = _xcBanc(1);
|
---|
| 365 | _params[1]->xx = _xcBanc(2);
|
---|
| 366 | _params[2]->xx = _xcBanc(3);
|
---|
[2073] | 367 |
|
---|
[2076] | 368 | _QQ(1,1) += sig_crd_p * sig_crd_p;
|
---|
| 369 | _QQ(2,2) += sig_crd_p * sig_crd_p;
|
---|
| 370 | _QQ(3,3) += sig_crd_p * sig_crd_p;
|
---|
| 371 | }
|
---|
[2073] | 372 |
|
---|
[2076] | 373 | // Receiver Clocks
|
---|
| 374 | // ---------------
|
---|
[2109] | 375 | _params[3]->xx = _xcBanc(4);
|
---|
[2073] | 376 | for (int iPar = 1; iPar <= _params.size(); iPar++) {
|
---|
| 377 | _QQ(iPar, 4) = 0.0;
|
---|
| 378 | }
|
---|
| 379 | _QQ(4,4) = sig_clk_0 * sig_clk_0;
|
---|
| 380 |
|
---|
[2084] | 381 | // Tropospheric Delay
|
---|
| 382 | // ------------------
|
---|
| 383 | if (_estTropo) {
|
---|
| 384 | _QQ(5,5) += sig_trp_p * sig_trp_p;
|
---|
| 385 | }
|
---|
[2073] | 386 | }
|
---|
| 387 |
|
---|
[2060] | 388 | // Update Step of the Filter (currently just a single-epoch solution)
|
---|
| 389 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 390 | t_irc bncModel::update(t_epoData* epoData) {
|
---|
| 391 |
|
---|
[2112] | 392 | SymmetricMatrix QQsav;
|
---|
[2109] | 393 | ColumnVector dx;
|
---|
[2112] | 394 | ColumnVector vv;
|
---|
[2073] | 395 |
|
---|
[2108] | 396 | do {
|
---|
[2060] | 397 |
|
---|
[2108] | 398 | if (epoData->size() < MINOBS) {
|
---|
| 399 | return failure;
|
---|
| 400 | }
|
---|
| 401 |
|
---|
| 402 | // Bancroft Solution
|
---|
| 403 | // -----------------
|
---|
| 404 | if (cmpBancroft(epoData) != success) {
|
---|
| 405 | return failure;
|
---|
| 406 | }
|
---|
[2080] | 407 |
|
---|
[2108] | 408 | // Status Prediction
|
---|
| 409 | // -----------------
|
---|
| 410 | predict(epoData);
|
---|
| 411 |
|
---|
[2109] | 412 | // Create First-Design Matrix
|
---|
| 413 | // --------------------------
|
---|
[2108] | 414 | unsigned nPar = _params.size();
|
---|
| 415 | unsigned nObs = _usePhase ? 2 * epoData->size() : epoData->size();
|
---|
| 416 |
|
---|
| 417 | Matrix AA(nObs, nPar); // first design matrix
|
---|
| 418 | ColumnVector ll(nObs); // tems observed-computed
|
---|
| 419 | SymmetricMatrix PP(nObs); PP = 0.0;
|
---|
| 420 |
|
---|
| 421 | unsigned iObs = 0;
|
---|
| 422 | QMapIterator<QString, t_satData*> itObs(epoData->satData);
|
---|
| 423 | while (itObs.hasNext()) {
|
---|
[2083] | 424 | ++iObs;
|
---|
[2108] | 425 | itObs.next();
|
---|
| 426 | QString prn = itObs.key();
|
---|
| 427 | t_satData* satData = itObs.value();
|
---|
| 428 |
|
---|
| 429 | double rhoCmp = cmpValue(satData);
|
---|
| 430 |
|
---|
| 431 | ll(iObs) = satData->P3 - rhoCmp;
|
---|
| 432 | PP(iObs,iObs) = 1.0 / (sig_P3 * sig_P3);
|
---|
[2083] | 433 | for (int iPar = 1; iPar <= _params.size(); iPar++) {
|
---|
[2108] | 434 | AA(iObs, iPar) = _params[iPar-1]->partial(satData, "");
|
---|
[2083] | 435 | }
|
---|
[2108] | 436 |
|
---|
| 437 | if (_usePhase) {
|
---|
| 438 | ++iObs;
|
---|
| 439 | ll(iObs) = satData->L3 - rhoCmp;
|
---|
| 440 | PP(iObs,iObs) = 1.0 / (sig_L3 * sig_L3);
|
---|
| 441 | for (int iPar = 1; iPar <= _params.size(); iPar++) {
|
---|
| 442 | if (_params[iPar-1]->type == bncParam::AMB_L3 &&
|
---|
| 443 | _params[iPar-1]->prn == prn) {
|
---|
[2109] | 444 | ll(iObs) -= _params[iPar-1]->xx;
|
---|
[2108] | 445 | }
|
---|
| 446 | AA(iObs, iPar) = _params[iPar-1]->partial(satData, prn);
|
---|
| 447 | }
|
---|
| 448 | }
|
---|
[2080] | 449 | }
|
---|
[2108] | 450 |
|
---|
[2112] | 451 | // Compute Filter Update
|
---|
[2108] | 452 | // ---------------------
|
---|
[2112] | 453 | QQsav = _QQ;
|
---|
| 454 |
|
---|
| 455 | Matrix ATP = AA.t() * PP;
|
---|
[2109] | 456 | SymmetricMatrix NN = _QQ.i();
|
---|
[2112] | 457 | NN << NN + ATP * AA;
|
---|
| 458 | _QQ = NN.i();
|
---|
| 459 | dx = _QQ * ATP * ll;
|
---|
| 460 | vv = ll - AA * dx;
|
---|
[2060] | 461 |
|
---|
[2112] | 462 | } while (outlierDetection(QQsav, vv, epoData->satData) != 0);
|
---|
[2111] | 463 |
|
---|
[2109] | 464 | // Set Solution Vector
|
---|
| 465 | // -------------------
|
---|
[2073] | 466 | QVectorIterator<bncParam*> itPar(_params);
|
---|
[2060] | 467 | while (itPar.hasNext()) {
|
---|
| 468 | bncParam* par = itPar.next();
|
---|
[2109] | 469 | par->xx += dx(par->index);
|
---|
[2060] | 470 | }
|
---|
| 471 |
|
---|
| 472 | return success;
|
---|
| 473 | }
|
---|
[2112] | 474 |
|
---|
| 475 | // Outlier Detection
|
---|
| 476 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 477 | int bncModel::outlierDetection(const SymmetricMatrix& QQsav,
|
---|
| 478 | const ColumnVector& vv,
|
---|
| 479 | QMap<QString, t_satData*>& satData) {
|
---|
| 480 |
|
---|
| 481 | const static double MAXRES_CODE = 10.0;
|
---|
| 482 | const static double MAXRES_PHASE = 0.10;
|
---|
| 483 |
|
---|
| 484 | double vvMaxCode = 0.0;
|
---|
| 485 | double vvMaxPhase = 0.0;
|
---|
| 486 | QMutableMapIterator<QString, t_satData*> itMaxCode(satData);
|
---|
| 487 | QMutableMapIterator<QString, t_satData*> itMaxPhase(satData);
|
---|
| 488 |
|
---|
| 489 | int ii = 0;
|
---|
| 490 | QMutableMapIterator<QString, t_satData*> it(satData);
|
---|
| 491 | while (it.hasNext()) {
|
---|
| 492 | it.next();
|
---|
| 493 | ++ii;
|
---|
| 494 |
|
---|
| 495 | if (vvMaxCode == 0.0 || fabs(vv(ii)) > vvMaxCode) {
|
---|
| 496 | vvMaxCode = fabs(vv(ii));
|
---|
| 497 | itMaxCode = it;
|
---|
| 498 | }
|
---|
| 499 |
|
---|
| 500 | if (_usePhase) {
|
---|
| 501 | ++ii;
|
---|
| 502 | if (vvMaxPhase == 0.0 || fabs(vv(ii)) > vvMaxPhase) {
|
---|
| 503 | vvMaxPhase = fabs(vv(ii));
|
---|
| 504 | itMaxPhase = it;
|
---|
| 505 | }
|
---|
| 506 | }
|
---|
| 507 | }
|
---|
| 508 |
|
---|
| 509 | if (vvMaxCode > MAXRES_CODE) {
|
---|
| 510 | QString prn = itMaxCode.key();
|
---|
| 511 | t_satData* satData = itMaxCode.value();
|
---|
| 512 | delete satData;
|
---|
| 513 | itMaxCode.remove();
|
---|
| 514 | _QQ = QQsav;
|
---|
| 515 | cout << "Code " << prn.toAscii().data() << " " << vvMaxCode << endl;
|
---|
| 516 | return 1;
|
---|
| 517 | }
|
---|
| 518 | else if (vvMaxPhase > MAXRES_PHASE) {
|
---|
| 519 | QString prn = itMaxPhase.key();
|
---|
| 520 | t_satData* satData = itMaxPhase.value();
|
---|
| 521 | delete satData;
|
---|
| 522 | itMaxPhase.remove();
|
---|
| 523 | _QQ = QQsav;
|
---|
| 524 | cout << "Phase " << prn.toAscii().data() << " " << vvMaxPhase << endl;
|
---|
| 525 | return 1;
|
---|
| 526 | }
|
---|
| 527 |
|
---|
| 528 | return 0;
|
---|
| 529 | }
|
---|