Index: trunk/BNC/src/PPP/pppModel.cpp =================================================================== --- trunk/BNC/src/PPP/pppModel.cpp (revision 5806) +++ trunk/BNC/src/PPP/pppModel.cpp (revision 5807) @@ -7,7 +7,4 @@ using namespace BNC; using namespace std; - -double Frac (double x) { return x-floor(x); }; -double Modulo (double x, double y) { return y*Frac(x/y); } Matrix t_astro::rotX(double Angle) { Index: trunk/BNC/src/bncmodel.cpp =================================================================== --- trunk/BNC/src/bncmodel.cpp (revision 5806) +++ trunk/BNC/src/bncmodel.cpp (revision 5807) @@ -906,53 +906,4 @@ } -// -////////////////////////////////////////////////////////////////////////////// -void bncModel::kalman(const Matrix& AA, const ColumnVector& ll, - const DiagonalMatrix& PP, - SymmetricMatrix& QQ, ColumnVector& dx) { - - Tracer tracer("bncModel::kalman"); - - int nPar = AA.Ncols(); -#if 1 - int nObs = AA.Nrows(); - UpperTriangularMatrix SS = Cholesky(QQ).t(); - - Matrix SA = SS*AA.t(); - Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0; - for (int ii = 1; ii <= nObs; ++ii) { - SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii)); - } - - SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA; - SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS; - - UpperTriangularMatrix UU; - QRZ(SRF, UU); - - SS = UU.SymSubMatrix(nObs+1, nObs+nPar); - UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs); - Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar); - - UpperTriangularMatrix SHi = SH_rt.i(); - - Matrix KT = SHi * YY; - SymmetricMatrix Hi; Hi << SHi * SHi.t(); - - dx = KT.t() * ll; - QQ << (SS.t() * SS); -#else - DiagonalMatrix Ql = PP.i(); - Matrix DD = QQ * AA.t(); - SymmetricMatrix SM(nPar); SM << AA * DD + Ql; - UpperTriangularMatrix UU = Cholesky(SM).t(); - UpperTriangularMatrix Ui = UU.i(); - Matrix EE = DD * Ui; - Matrix KK = EE * Ui.t(); - QQ << QQ - EE * EE.t(); - dx = KK * ll; -#endif -} - // Phase Wind-Up Correction /////////////////////////////////////////////////////////////////////////// Index: trunk/BNC/src/bncmodel.h =================================================================== --- trunk/BNC/src/bncmodel.h (revision 5806) +++ trunk/BNC/src/bncmodel.h (revision 5807) @@ -98,8 +98,4 @@ } - static void kalman(const Matrix& AA, const ColumnVector& ll, - const DiagonalMatrix& PP, - SymmetricMatrix& QQ, ColumnVector& dx); - static double delay_saast(const ColumnVector& xyz, double Ele); Index: trunk/BNC/src/bncutils.cpp =================================================================== --- trunk/BNC/src/bncutils.cpp (revision 5806) +++ trunk/BNC/src/bncutils.cpp (revision 5807) @@ -46,4 +46,6 @@ #include #include + +#include #include "bncutils.h" @@ -285,4 +287,16 @@ } +// +//////////////////////////////////////////////////////////////////////////// +double Frac (double x) { + return x-floor(x); +} + +// +//////////////////////////////////////////////////////////////////////////// +double Modulo (double x, double y) { + return y*Frac(x/y); +} + // Round to nearest integer //////////////////////////////////////////////////////////////////////////// @@ -548,2 +562,39 @@ } } + +// +////////////////////////////////////////////////////////////////////////////// +void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP, + SymmetricMatrix& QQ, ColumnVector& dx) { + + Tracer tracer("kalman"); + + int nPar = AA.Ncols(); + int nObs = AA.Nrows(); + UpperTriangularMatrix SS = Cholesky(QQ).t(); + + Matrix SA = SS*AA.t(); + Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0; + for (int ii = 1; ii <= nObs; ++ii) { + SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii)); + } + + SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA; + SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS; + + UpperTriangularMatrix UU; + QRZ(SRF, UU); + + SS = UU.SymSubMatrix(nObs+1, nObs+nPar); + UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs); + Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar); + + UpperTriangularMatrix SHi = SH_rt.i(); + + Matrix KT = SHi * YY; + SymmetricMatrix Hi; Hi << SHi * SHi.t(); + + dx = KT.t() * ll; + QQ << (SS.t() * SS); +} + Index: trunk/BNC/src/bncutils.h =================================================================== --- trunk/BNC/src/bncutils.h (revision 5806) +++ trunk/BNC/src/bncutils.h (revision 5807) @@ -34,65 +34,68 @@ #include -void expandEnvVar(QString& str); +void expandEnvVar(QString& str); -QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks); +QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks); -void currentGPSWeeks(int& week, double& sec); +void currentGPSWeeks(int& week, double& sec); -QDateTime currentDateAndTimeGPS(); +QDateTime currentDateAndTimeGPS(); -QByteArray ggaString(const QByteArray& latitude, - const QByteArray& longitude, - const QByteArray& height); +QByteArray ggaString(const QByteArray& latitude, const QByteArray& longitude, + const QByteArray& height); -void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv, - const ColumnVector& rsw, ColumnVector& xyz); +void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv, + const ColumnVector& rsw, ColumnVector& xyz); -void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv, - const ColumnVector& xyz, ColumnVector& rsw); +void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv, + const ColumnVector& xyz, ColumnVector& rsw); -t_irc xyz2ell(const double* XYZ, double* Ell); +t_irc xyz2ell(const double* XYZ, double* Ell); -void xyz2neu(const double* Ell, const double* xyz, double* neu); +void xyz2neu(const double* Ell, const double* xyz, double* neu); -void neu2xyz(const double* Ell, const double* neu, double* xyz); +void neu2xyz(const double* Ell, const double* neu, double* xyz); -void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi); +void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi); -void jacobiEll_XYZ(const double* Ell, Matrix& jacobi); +void jacobiEll_XYZ(const double* Ell, Matrix& jacobi); -void covariXYZ_NEU(const SymmetricMatrix& Qxyz, const double* Ell, - SymmetricMatrix& Qneu); +void covariXYZ_NEU(const SymmetricMatrix& Qxyz, const double* Ell, + SymmetricMatrix& Qneu); -void covariNEU_XYZ(const SymmetricMatrix& Qneu, const double* Ell, - SymmetricMatrix& Qxyz); +void covariNEU_XYZ(const SymmetricMatrix& Qneu, const double* Ell, + SymmetricMatrix& Qxyz); -double nint(double val); +double Frac(double x); -ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx, - double* acc, - ColumnVector (*der)(double x, const ColumnVector& y, double* acc)); +double Modulo(double x, double y); -void GPSweekFromDateAndTime(const QDateTime& dateTime, - int& GPSWeek, double& GPSWeeks); +double nint(double val); -void GPSweekFromYMDhms(int year, int month, int day, int hour, int min, - double sec, int& GPSWeek, double& GPSWeeks); +ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx, double* acc, + ColumnVector (*der)(double x, const ColumnVector& y, double* acc)); -void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac); +void GPSweekFromDateAndTime(const QDateTime& dateTime, int& GPSWeek, double& GPSWeeks); -bool findInVector(const std::vector& vv, const QString& str); +void GPSweekFromYMDhms(int year, int month, int day, int hour, int min, double sec, + int& GPSWeek, double& GPSWeeks); -int readInt(const QString& str, int pos, int len, int& value); +void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac); -int readDbl(const QString& str, int pos, int len, double& value); +bool findInVector(const std::vector& vv, const QString& str); -void topos(double xRec, double yRec, double zRec, - double xSat, double ySat, double zSat, - double& rho, double& eleSat, double& azSat); +int readInt(const QString& str, int pos, int len, int& value); -void deg2DMS(double decDeg, int& deg, int& min, double& sec); +int readDbl(const QString& str, int pos, int len, double& value); -QString fortranFormat(double value, int width, int prec); +void topos(double xRec, double yRec, double zRec, double xSat, double ySat, double zSat, + double& rho, double& eleSat, double& azSat); + +void deg2DMS(double decDeg, int& deg, int& min, double& sec); + +QString fortranFormat(double value, int width, int prec); + +void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP, + SymmetricMatrix& QQ, ColumnVector& dx); #endif