Changeset 6089 in ntrip for trunk/BNC/src/PPP_free

Timestamp:

Sep 7, 2014, 7:06:54 PM (10 years ago)

Author:

mervart

Message:

 

Location:

trunk/BNC/src/PPP_free

Files:

• bancroft.cpp (deleted)
• bancroft.h (deleted)
• bncmodel.cpp (modified) (1 diff)
• bncmodel.h (modified) (1 diff)

trunk/BNC/src/PPP_free/bncmodel.cpp

@@ -1201 +1201 @@
   return failure;
 }
+
+// 
+////////////////////////////////////////////////////////////////////////////
+double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
+  return aa(1)*bb(1) +  aa(2)*bb(2) +  aa(3)*bb(3) -  aa(4)*bb(4);
+}
+
+// 
+////////////////////////////////////////////////////////////////////////////
+void bncModel::bancroft(const Matrix& BBpass, ColumnVector& pos) {
+
+  if (pos.Nrows() != 4) {
+    pos.ReSize(4);
+  }
+  pos = 0.0;
+
+  for (int iter = 1; iter <= 2; iter++) {
+    Matrix BB = BBpass;
+    int mm = BB.Nrows();
+    for (int ii = 1; ii <= mm; ii++) {
+      double xx = BB(ii,1);
+      double yy = BB(ii,2);
+      double traveltime = 0.072;
+      if (iter > 1) {
+        double zz  = BB(ii,3);
+        double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) + 
+                           (yy-pos(2)) * (yy-pos(2)) + 
+                           (zz-pos(3)) * (zz-pos(3)) );
+        traveltime = rho / t_CST::c;
+      }
+      double angle = traveltime * t_CST::omega;
+      double cosa  = cos(angle);
+      double sina  = sin(angle);
+      BB(ii,1) =  cosa * xx + sina * yy;
+      BB(ii,2) = -sina * xx + cosa * yy;
+    }
+    
+    Matrix BBB;
+    if (mm > 4) {
+      SymmetricMatrix hlp; hlp << BB.t() * BB;
+      BBB = hlp.i() * BB.t();
+    }
+    else {
+      BBB = BB.i();
+    }
+    ColumnVector ee(mm); ee = 1.0;
+    ColumnVector alpha(mm); alpha = 0.0;
+    for (int ii = 1; ii <= mm; ii++) {
+      alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0; 
+    }
+    ColumnVector BBBe     = BBB * ee;
+    ColumnVector BBBalpha = BBB * alpha;
+    double aa = lorentz(BBBe, BBBe);
+    double bb = lorentz(BBBe, BBBalpha)-1;
+    double cc = lorentz(BBBalpha, BBBalpha);
+    double root = sqrt(bb*bb-aa*cc);
+
+    Matrix hlpPos(4,2); 
+    hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
+    hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
+
+    ColumnVector omc(2);
+    for (int pp = 1; pp <= 2; pp++) {
+      hlpPos(4,pp)      = -hlpPos(4,pp);
+      omc(pp) = BB(1,4) - 
+                sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
+                      (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
+                      (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) - 
+                hlpPos(4,pp);
+    }
+    if ( fabs(omc(1)) > fabs(omc(2)) ) {
+      pos = hlpPos.Column(2);
+    }
+    else {
+      pos = hlpPos.Column(1);
+    }
+  }
+}
+

trunk/BNC/src/PPP_free/bncmodel.h

@@ -134 +134 @@
                          QMap<QString, t_satData*>& satData);
 
+  void bancroft(const Matrix& BBpass, ColumnVector& pos);
+
   class pppPos {
    public: