Changeset 5807 in ntrip


Ignore:
Timestamp:
Aug 6, 2014, 11:09:26 AM (10 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC/src
Files:
5 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src/PPP/pppModel.cpp

    r5805 r5807  
    77using namespace BNC;
    88using namespace std;
    9 
    10 double Frac (double x) { return x-floor(x); };
    11 double Modulo (double x, double y) { return y*Frac(x/y); }
    129
    1310Matrix t_astro::rotX(double Angle) {

  • trunk/BNC/src/bncmodel.cpp

    r5805 r5807  
    906906}
    907907
    908 //
    909 //////////////////////////////////////////////////////////////////////////////
    910 void bncModel::kalman(const Matrix& AA, const ColumnVector& ll,
    911                       const DiagonalMatrix& PP,
    912                       SymmetricMatrix& QQ, ColumnVector& dx) {
    913 
    914   Tracer tracer("bncModel::kalman");
    915 
    916   int nPar = AA.Ncols();
    917 #if 1
    918   int nObs = AA.Nrows();
    919   UpperTriangularMatrix SS = Cholesky(QQ).t();
    920 
    921   Matrix SA = SS*AA.t();
    922   Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
    923   for (int ii = 1; ii <= nObs; ++ii) {
    924     SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
    925   }
    926 
    927   SRF.SubMatrix   (nObs+1, nObs+nPar, 1, nObs) = SA;
    928   SRF.SymSubMatrix(nObs+1, nObs+nPar)          = SS;
    929  
    930   UpperTriangularMatrix UU;
    931   QRZ(SRF, UU);
    932  
    933   SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
    934   UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
    935   Matrix YY  = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
    936  
    937   UpperTriangularMatrix SHi = SH_rt.i();
    938  
    939   Matrix KT  = SHi * YY;
    940   SymmetricMatrix Hi; Hi << SHi * SHi.t();
    941 
    942   dx = KT.t() * ll;
    943   QQ << (SS.t() * SS);
    944 #else
    945   DiagonalMatrix        Ql = PP.i();
    946   Matrix                DD = QQ * AA.t();
    947   SymmetricMatrix       SM(nPar); SM << AA * DD + Ql;
    948   UpperTriangularMatrix UU = Cholesky(SM).t();
    949   UpperTriangularMatrix Ui = UU.i();
    950   Matrix                EE = DD * Ui;
    951   Matrix                KK = EE * Ui.t();
    952   QQ << QQ - EE * EE.t();
    953   dx = KK * ll;
    954 #endif
    955 }
    956 
    957908// Phase Wind-Up Correction
    958909///////////////////////////////////////////////////////////////////////////

  • trunk/BNC/src/bncmodel.h

    r5805 r5807  
    9898  }
    9999
    100   static void kalman(const Matrix& AA, const ColumnVector& ll,
    101                      const DiagonalMatrix& PP,
    102                      SymmetricMatrix& QQ, ColumnVector& dx);
    103 
    104100  static double delay_saast(const ColumnVector& xyz, double Ele);
    105101

  • trunk/BNC/src/bncutils.cpp

    r5753 r5807  
    4646#include <QStringList>
    4747#include <QDateTime>
     48
     49#include <newmatap.h>
    4850
    4951#include "bncutils.h"
    … …  
    285287}
    286288
     289//
     290////////////////////////////////////////////////////////////////////////////
     291double Frac (double x) {
     292  return x-floor(x);
     293}
     294
     295//
     296////////////////////////////////////////////////////////////////////////////
     297double Modulo (double x, double y) {
     298  return y*Frac(x/y);
     299}
     300
    287301// Round to nearest integer
    288302////////////////////////////////////////////////////////////////////////////
    … …  
    548562  }
    549563}
     564
     565//
     566//////////////////////////////////////////////////////////////////////////////
     567void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP,
     568            SymmetricMatrix& QQ, ColumnVector& dx) {
     569
     570  Tracer tracer("kalman");
     571
     572  int nPar = AA.Ncols();
     573  int nObs = AA.Nrows();
     574  UpperTriangularMatrix SS = Cholesky(QQ).t();
     575
     576  Matrix SA = SS*AA.t();
     577  Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
     578  for (int ii = 1; ii <= nObs; ++ii) {
     579    SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
     580  }
     581
     582  SRF.SubMatrix   (nObs+1, nObs+nPar, 1, nObs) = SA;
     583  SRF.SymSubMatrix(nObs+1, nObs+nPar)          = SS;
     584 
     585  UpperTriangularMatrix UU;
     586  QRZ(SRF, UU);
     587 
     588  SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
     589  UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
     590  Matrix YY  = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
     591 
     592  UpperTriangularMatrix SHi = SH_rt.i();
     593 
     594  Matrix KT  = SHi * YY;
     595  SymmetricMatrix Hi; Hi << SHi * SHi.t();
     596
     597  dx = KT.t() * ll;
     598  QQ << (SS.t() * SS);
     599}
     600

  • trunk/BNC/src/bncutils.h

    r5753 r5807  
    3434#include <bncconst.h>
    3535
    36 void expandEnvVar(QString& str);
     36void         expandEnvVar(QString& str);
    3737
    38 QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks);
     38QDateTime    dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks);
    3939
    40 void currentGPSWeeks(int& week, double& sec);
     40void         currentGPSWeeks(int& week, double& sec);
    4141
    42 QDateTime currentDateAndTimeGPS();
     42QDateTime    currentDateAndTimeGPS();
    4343
    44 QByteArray ggaString(const QByteArray& latitude,
    45                      const QByteArray& longitude,
    46                      const QByteArray& height);
     44QByteArray   ggaString(const QByteArray& latitude, const QByteArray& longitude,
     45                       const QByteArray& height);
    4746
    48 void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv,
    49                 const ColumnVector& rsw, ColumnVector& xyz);
     47void         RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv,
     48                        const ColumnVector& rsw, ColumnVector& xyz);
    5049
    51 void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
    52                 const ColumnVector& xyz, ColumnVector& rsw);
     50void         XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
     51                        const ColumnVector& xyz, ColumnVector& rsw);
    5352
    54 t_irc xyz2ell(const double* XYZ, double* Ell);
     53t_irc        xyz2ell(const double* XYZ, double* Ell);
    5554
    56 void xyz2neu(const double* Ell, const double* xyz, double* neu);
     55void         xyz2neu(const double* Ell, const double* xyz, double* neu);
    5756
    58 void neu2xyz(const double* Ell, const double* neu, double* xyz);
     57void         neu2xyz(const double* Ell, const double* neu, double* xyz);
    5958
    60 void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi);
     59void         jacobiXYZ_NEU(const double* Ell, Matrix& jacobi);
    6160
    62 void jacobiEll_XYZ(const double* Ell, Matrix& jacobi);
     61void         jacobiEll_XYZ(const double* Ell, Matrix& jacobi);
    6362
    64 void covariXYZ_NEU(const SymmetricMatrix& Qxyz, const double* Ell,
    65                    SymmetricMatrix& Qneu);
     63void         covariXYZ_NEU(const SymmetricMatrix& Qxyz, const double* Ell,
     64                           SymmetricMatrix& Qneu);
    6665
    67 void covariNEU_XYZ(const SymmetricMatrix& Qneu, const double* Ell,
    68                    SymmetricMatrix& Qxyz);
     66void         covariNEU_XYZ(const SymmetricMatrix& Qneu, const double* Ell,
     67                           SymmetricMatrix& Qxyz);
    6968
    70 double nint(double val);
     69double       Frac(double x);
    7170
    72 ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx,
    73                          double* acc,
    74             ColumnVector (*der)(double x, const ColumnVector& y, double* acc));
     71double       Modulo(double x, double y);
    7572
    76 void GPSweekFromDateAndTime(const QDateTime& dateTime,
    77                             int& GPSWeek, double& GPSWeeks);
     73double       nint(double val);
    7874
    79 void GPSweekFromYMDhms(int year, int month, int day, int hour, int min,
    80                        double sec, int& GPSWeek, double& GPSWeeks);
     75ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx, double* acc,
     76                         ColumnVector (*der)(double x, const ColumnVector& y, double* acc));
    8177
    82 void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac);
     78void         GPSweekFromDateAndTime(const QDateTime& dateTime, int& GPSWeek, double& GPSWeeks);
    8379
    84 bool findInVector(const std::vector<QString>& vv, const QString& str);
     80void         GPSweekFromYMDhms(int year, int month, int day, int hour, int min, double sec,
     81                               int& GPSWeek, double& GPSWeeks);
    8582
    86 int readInt(const QString& str, int pos, int len, int& value);
     83void         mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac);
    8784
    88 int readDbl(const QString& str, int pos, int len, double& value);
     85bool         findInVector(const std::vector<QString>& vv, const QString& str);
    8986
    90 void topos(double xRec, double yRec, double zRec,
    91            double xSat, double ySat, double zSat,
    92            double& rho, double& eleSat, double& azSat);
     87int          readInt(const QString& str, int pos, int len, int& value);
    9388
    94 void deg2DMS(double decDeg, int& deg, int& min, double& sec);
     89int          readDbl(const QString& str, int pos, int len, double& value);
    9590
    96 QString fortranFormat(double value, int width, int prec);
     91void         topos(double xRec, double yRec, double zRec, double xSat, double ySat, double zSat,
     92                   double& rho, double& eleSat, double& azSat);
     93
     94void         deg2DMS(double decDeg, int& deg, int& min, double& sec);
     95
     96QString      fortranFormat(double value, int width, int prec);
     97
     98void         kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP,
     99                    SymmetricMatrix& QQ, ColumnVector& dx);
    97100
    98101#endif
Note: See TracChangeset for help on using the changeset viewer.