Changeset 6089 in ntrip for trunk/BNC/src/PPP_free/bncmodel.cpp


Ignore:
Timestamp:
Sep 7, 2014, 7:06:54 PM (8 years ago)
Author:
mervart
Message:
 
File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/src/PPP_free/bncmodel.cpp

    r6086 r6089  
    12011201  return failure;
    12021202}
     1203
     1204//
     1205////////////////////////////////////////////////////////////////////////////
     1206double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
     1207  return aa(1)*bb(1) +  aa(2)*bb(2) +  aa(3)*bb(3) -  aa(4)*bb(4);
     1208}
     1209
     1210//
     1211////////////////////////////////////////////////////////////////////////////
     1212void bncModel::bancroft(const Matrix& BBpass, ColumnVector& pos) {
     1213
     1214  if (pos.Nrows() != 4) {
     1215    pos.ReSize(4);
     1216  }
     1217  pos = 0.0;
     1218
     1219  for (int iter = 1; iter <= 2; iter++) {
     1220    Matrix BB = BBpass;
     1221    int mm = BB.Nrows();
     1222    for (int ii = 1; ii <= mm; ii++) {
     1223      double xx = BB(ii,1);
     1224      double yy = BB(ii,2);
     1225      double traveltime = 0.072;
     1226      if (iter > 1) {
     1227        double zz  = BB(ii,3);
     1228        double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
     1229                           (yy-pos(2)) * (yy-pos(2)) +
     1230                           (zz-pos(3)) * (zz-pos(3)) );
     1231        traveltime = rho / t_CST::c;
     1232      }
     1233      double angle = traveltime * t_CST::omega;
     1234      double cosa  = cos(angle);
     1235      double sina  = sin(angle);
     1236      BB(ii,1) =  cosa * xx + sina * yy;
     1237      BB(ii,2) = -sina * xx + cosa * yy;
     1238    }
     1239   
     1240    Matrix BBB;
     1241    if (mm > 4) {
     1242      SymmetricMatrix hlp; hlp << BB.t() * BB;
     1243      BBB = hlp.i() * BB.t();
     1244    }
     1245    else {
     1246      BBB = BB.i();
     1247    }
     1248    ColumnVector ee(mm); ee = 1.0;
     1249    ColumnVector alpha(mm); alpha = 0.0;
     1250    for (int ii = 1; ii <= mm; ii++) {
     1251      alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
     1252    }
     1253    ColumnVector BBBe     = BBB * ee;
     1254    ColumnVector BBBalpha = BBB * alpha;
     1255    double aa = lorentz(BBBe, BBBe);
     1256    double bb = lorentz(BBBe, BBBalpha)-1;
     1257    double cc = lorentz(BBBalpha, BBBalpha);
     1258    double root = sqrt(bb*bb-aa*cc);
     1259
     1260    Matrix hlpPos(4,2);
     1261    hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
     1262    hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
     1263
     1264    ColumnVector omc(2);
     1265    for (int pp = 1; pp <= 2; pp++) {
     1266      hlpPos(4,pp)      = -hlpPos(4,pp);
     1267      omc(pp) = BB(1,4) -
     1268                sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
     1269                      (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
     1270                      (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
     1271                hlpPos(4,pp);
     1272    }
     1273    if ( fabs(omc(1)) > fabs(omc(2)) ) {
     1274      pos = hlpPos.Column(2);
     1275    }
     1276    else {
     1277      pos = hlpPos.Column(1);
     1278    }
     1279  }
     1280}
     1281
Note: See TracChangeset for help on using the changeset viewer.