Index: trunk/BNC/bncmodel.cpp =================================================================== --- trunk/BNC/bncmodel.cpp (revision 2072) +++ trunk/BNC/bncmodel.cpp (revision 2073) @@ -52,9 +52,15 @@ const unsigned MINOBS = 4; const double MINELE = 10.0 * M_PI / 180.0; +const double sig_crd_0 = 100.0; +const double sig_crd_p = 100.0; +const double sig_clk_0 = 1000.0; // Constructor //////////////////////////////////////////////////////////////////////////// -bncParam::bncParam(bncParam::parType typeIn) { - type = typeIn; +bncParam::bncParam(bncParam::parType typeIn, int indexIn) { + type = typeIn; + index = indexIn; + x0 = 0.0; + xx = 0.0; } @@ -86,9 +92,21 @@ bncModel::bncModel() { _xcBanc.ReSize(4); _xcBanc = 0.0; - _params.push_back(new bncParam(bncParam::CRD_X)); - _params.push_back(new bncParam(bncParam::CRD_Y)); - _params.push_back(new bncParam(bncParam::CRD_Z)); - _params.push_back(new bncParam(bncParam::RECCLK)); + _params.push_back(new bncParam(bncParam::CRD_X, 1)); + _params.push_back(new bncParam(bncParam::CRD_Y, 2)); + _params.push_back(new bncParam(bncParam::CRD_Z, 3)); + _params.push_back(new bncParam(bncParam::RECCLK, 4)); _ellBanc.ReSize(3); + + unsigned nPar = _params.size(); + _QQ.ReSize(nPar); + _QQ = 0.0; + + _QQ(1,1) = sig_crd_0 * sig_crd_0; + _QQ(2,2) = sig_crd_0 * sig_crd_0; + _QQ(3,3) = sig_crd_0 * sig_crd_0; + _QQ(4,4) = sig_clk_0 * sig_clk_0; + + _xx.ReSize(nPar); + _xx = 0.0; } @@ -123,23 +141,4 @@ bancroft(BB, _xcBanc); - // Set Parameter A Priori Values - // ----------------------------- - QListIterator itPar(_params); - while (itPar.hasNext()) { - bncParam* par = itPar.next(); - if (par->type == bncParam::CRD_X) { - par->x0 = _xcBanc(1); - } - else if (par->type == bncParam::CRD_Y) { - par->x0 = _xcBanc(2); - } - else if (par->type == bncParam::CRD_Z) { - par->x0 = _xcBanc(3); - } - else if (par->type == bncParam::RECCLK) { - par->x0 = _xcBanc(4); - } - } - // Ellipsoidal Coordinates // ------------------------ @@ -179,11 +178,15 @@ double bncModel::cmpValueP3(t_satData* satData) { - double rho0 = (satData->xx - _xcBanc.Rows(1,3)).norm_Frobenius(); - ColumnVector xRec(3); + xRec(1) = x(); + xRec(2) = y(); + xRec(3) = z(); + + double rho0 = (satData->xx - xRec).norm_Frobenius(); double dPhi = t_CST::omega * rho0 / t_CST::c; - xRec(1) = _xcBanc(1) * cos(dPhi) - _xcBanc(2) * sin(dPhi); - xRec(2) = _xcBanc(2) * cos(dPhi) + _xcBanc(1) * sin(dPhi); - xRec(3) = _xcBanc(3); + + xRec(1) = x() * cos(dPhi) - y() * sin(dPhi); + xRec(2) = y() * cos(dPhi) + x() * sin(dPhi); + xRec(3) = z(); satData->rho = (satData->xx - xRec).norm_Frobenius(); @@ -191,5 +194,5 @@ double tropDelay = delay_saast(satData->eleSat); - return satData->rho + _xcBanc(4) - satData->clk + tropDelay; + return satData->rho + clk() - satData->clk + tropDelay; } @@ -227,4 +230,30 @@ } +// Prediction Step of the Filter +//////////////////////////////////////////////////////////////////////////// +void bncModel::predict() { + + _params[0]->x0 = _xcBanc(1); + _params[1]->x0 = _xcBanc(2); + _params[2]->x0 = _xcBanc(3); + _params[3]->x0 = _xcBanc(4); + + _params[0]->xx = 0.0; + _params[1]->xx = 0.0; + _params[2]->xx = 0.0; + _params[3]->xx = 0.0; + + _QQ(1,1) += sig_crd_p * sig_crd_p; + _QQ(2,2) += sig_crd_p * sig_crd_p; + _QQ(3,3) += sig_crd_p * sig_crd_p; + + for (int iPar = 1; iPar <= _params.size(); iPar++) { + _QQ(iPar, 4) = 0.0; + } + _QQ(4,4) = sig_clk_0 * sig_clk_0; + + _xx = 0.0; +} + // Update Step of the Filter (currently just a single-epoch solution) //////////////////////////////////////////////////////////////////////////// @@ -235,4 +264,6 @@ } + predict(); + unsigned nPar = _params.size(); unsigned nObs = epoData->size(); @@ -240,6 +271,6 @@ // Create First-Design Matrix // -------------------------- - _AA.ReSize(nObs, nPar); // variance-covariance matrix - _ll.ReSize(nObs); // tems observed-computed + Matrix AA(nObs, nPar); // first design matrix + ColumnVector ll(nObs); // tems observed-computed unsigned iObs = 0; @@ -250,35 +281,28 @@ QString prn = itObs.key(); t_satData* satData = itObs.value(); - _ll(iObs) = satData->P3 - cmpValueP3(satData); - - unsigned iPar = 0; - QListIterator itPar(_params); - while (itPar.hasNext()) { - ++iPar; - bncParam* par = itPar.next(); - _AA(iObs, iPar) = par->partialP3(satData); + ll(iObs) = satData->P3 - cmpValueP3(satData); + + for (int iPar = 1; iPar <= _params.size(); iPar++) { + AA(iObs, iPar) = _params[iPar-1]->partialP3(satData); } } - // Compute Least-Squares Solution - // ------------------------------ - _QQ.ReSize(nPar); - _QQ << _AA.t() * _AA; - _QQ = _QQ.i(); - _dx = _QQ * _AA.t() * _ll; - - // Compute Residuals - // ----------------- - ColumnVector vv = _AA * _dx - _ll; + // Compute Kalman Update + // --------------------- + IdentityMatrix PP(nObs); + SymmetricMatrix HH; HH << PP + AA * _QQ * AA.t(); + SymmetricMatrix Hi = HH.i(); + Matrix KK = _QQ * AA.t() * Hi; + ColumnVector v1 = ll - AA * _xx; + _xx = _xx + KK * v1; + IdentityMatrix Id(nPar); + _QQ << (Id - KK * AA) * _QQ; // Set Solution Vector // ------------------- - _xx.ReSize(nPar); - unsigned iPar = 0; - QListIterator itPar(_params); + QVectorIterator itPar(_params); while (itPar.hasNext()) { - ++iPar; bncParam* par = itPar.next(); - _xx(iPar) = par->x0 + _dx(iPar); + par->xx = _xx(par->index); } Index: trunk/BNC/bncmodel.h =================================================================== --- trunk/BNC/bncmodel.h (revision 2072) +++ trunk/BNC/bncmodel.h (revision 2073) @@ -37,9 +37,16 @@ public: enum parType {CRD_X, CRD_Y, CRD_Z, RECCLK, TROPO, AMB_L3}; - bncParam(parType typeIn); + bncParam(parType typeIn, int indexIn); ~bncParam(); double partialP3(t_satData* satData); + bool isCrd() const { + return (type == CRD_X || type == CRD_Y || type == CRD_Z); + } + double solVal() const {return x0 + xx;} + double aprVal() const {return x0;} parType type; + double xx; double x0; + int index; }; @@ -50,19 +57,19 @@ t_irc cmpBancroft(t_epoData* epoData); t_irc update(t_epoData* epoData); - const ColumnVector& xcBanc() const {return _xcBanc;} - const ColumnVector& xx() const {return _xx;} + double x() const {return _params[0]->solVal();} + double y() const {return _params[1]->solVal();} + double z() const {return _params[2]->solVal();} + double clk() const {return _params[3]->solVal();} private: double cmpValueP3(t_satData* satData); double delay_saast(double Ele); + void predict(); - QList _params; - SymmetricMatrix _QQ; - Matrix _AA; - ColumnVector _ll; - ColumnVector _dx; - ColumnVector _xx; - ColumnVector _xcBanc; - ColumnVector _ellBanc; + QVector _params; + SymmetricMatrix _QQ; + ColumnVector _xx; + ColumnVector _xcBanc; + ColumnVector _ellBanc; }; Index: trunk/BNC/bncpppclient.cpp =================================================================== --- trunk/BNC/bncpppclient.cpp (revision 2072) +++ trunk/BNC/bncpppclient.cpp (revision 2073) @@ -344,7 +344,7 @@ str << " PPP " << _staID.data() << " " << _epoData->tt.timestr(1) << " " << _epoData->size() << " " - << setw(14) << setprecision(3) << _model->xx()(1) << " " - << setw(14) << setprecision(3) << _model->xx()(2) << " " - << setw(14) << setprecision(3) << _model->xx()(3); + << setw(14) << setprecision(3) << _model->x() << " " + << setw(14) << setprecision(3) << _model->y() << " " + << setw(14) << setprecision(3) << _model->z(); emit newMessage(QString(str.str().c_str()).toAscii(), true);