ARLambda.cpp

Go to the documentation of this file.

#pragma ident "$Id: ARLambda.cpp 2620 2011-05-26 14:42:13Z yanweignss $"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Wei Yan - Chinese Academy of Sciences . 2011
//
//============================================================================

#include "ARLambda.hpp"


namespace gpstk
{
   Vector<double> ARLambda::resolveIntegerAmbiguity( 
                                               const Vector<double>& ambFloat, 
                                               const Matrix<double>& ambCov )
      throw(ARException)
   {
      // check input
      if( ambFloat.size()!=ambCov.rows() || ambFloat.size()!=ambCov.cols() )
      {
         ARException e("The dimension of input does not match.");
         GPSTK_THROW(e);
      }

      try
      {
         Matrix<double> F; Vector<double> S;
         if( lambda(ambFloat,ambCov,F,S,2)==0 )
         {
            Vector<double> ambFixed( ambFloat.size(), 0.0 );
            for(int i=0; i<ambFloat.size(); i++) 
            {
               ambFixed(i) = F(i,0);
            }

            squaredRatio = (S(0)<1e-12) ? 9999.9 : S(1)/S(0);
            
            return ambFixed;
         }
         else
         {
            ARException e("Failed to resolve the integer ambiguities.");
            GPSTK_THROW(e);
         }
      }
      catch(Exception& e)
      {
         GPSTK_RETHROW(e);
      }
      catch (...)
      {
         ARException e("Failed to resolve the integer ambiguities.");
         GPSTK_THROW(e);
      }
         // it should never go here, but we sill return the float ambiguities
      return ambFloat;

   }  // End of method 'ARMLambda::resolveIntegerAmbiguity()'

   
   int ARLambda::factorize( const Matrix<double>& Q, 
                            Matrix<double>& L, 
                            Vector<double>& D)
   {
      // TODO: CHECK UNEXPECTED INPUT
      // L:nxn Z:nxn 0<=(i,j)<n

      const int n = static_cast<int>(Q.rows());

      Matrix<double> QC(Q);
      L.resize(n, n, 0.0);
      D.resize(n, 0.0);

      for(int i = n-1; i >= 0; i--) 
      {
         D(i) = QC(i,i);
         if( D(i) <= 0.0 ) return -1;
         double temp = std::sqrt(D(i));
         for(int j=0; j<=i; j++) L(i,j) = QC(i,j)/temp;
         for(int j=0; j<=i-1; j++) 
         {
            for(int k=0; k<=j; k++) QC(j,k) -= L(i,k) * L(i,j);
         }
         for(int j=0; j<=i; j++) L(i,j) /= L(i,i);
      }

      return 0;

   }  // End of method 'ARLambda::factorize()'


   void ARLambda::gauss(Matrix<double>& L, Matrix<double>& Z, int i, int j)
   {
      //TODO: CHECK UNEXPECTED INPUT
      // L:nxn Z:nxn 0<=(i,j)<n

      const int n = L.rows();
      const int mu = (int)round(L(i,j));
      if(mu != 0) 
      {
         for(int k=i; k<n; k++) L(k,j) -= (double)mu*L(k,i);
         for(int k=0; k<n; k++) Z(k,j) -= (double)mu*Z(k,i);
      }
   }  // End of method 'ARLambda::gauss()'

   
   void ARLambda::permute( Matrix<double>& L, 
                           Vector<double>& D, 
                           int j, 
                           double del, 
                           Matrix<double>& Z )
   {  
      //TODO: CHECK UNEXPECTED INPUT
      // L:nxn D:nx1 Z:nxn 0<=j<n

      const int n = L.rows();

      double eta=D(j)/del;
      double lam=D(j+1)*L(j+1,j)/del;

      D[j]=eta*D(j+1); 
      D(j+1)=del;
      for(int k=0;k<=j-1;k++) 
      {
         double a0=L(j,k); 
         double a1=L(j+1,k);
         L(j,k) =-L(j+1,j)*a0 + a1;
         L(j+1,k) = eta*a0 + lam*a1;
      }
      L(j+1,j)=lam;
      for(int k=j+2; k<n; k++) swap(L(k,j),L(k,j+1));
      for(int k=0; k<n; k++) swap(Z(k,j),Z(k,j+1));

   }  // End of method 'ARLambda::permute()'

   
   void ARLambda::reduction( Matrix<double>& L, 
                             Vector<double>& D,
                             Matrix<double>& Z)
   {
      //TODO: CHECK UNEXPECTED INPUT
      // L:nxn D:nx1 Z:nxn 0<=j<n

      const int n = L.rows();

      int j(n-2), k(n-2);
      while(j>=0) 
      {
         if(j<=k)
         {
            for (int i=j+1; i<n; i++) 
            {
               gauss(L,Z,i,j);
            }
         } 

         double del=D(j)+L(j+1,j)*L(j+1,j)*D(j+1);

         if(del+1E-6<D(j+1)) 
         { 
            permute(L,D,j,del,Z);
            k=j; j=n-2;
         }
         else
         { 
            j--;
         }

      }  // end while

   }  // End of method 'ARLambda::reduction()'


   int ARLambda::search( const Matrix<double>& L, 
                         const Vector<double>& D, 
                         const Vector<double>& zs, 
                         Matrix<double>& zn, 
                         Vector<double>& s, 
                          const int& m )
   {

      ARException e("The LAMBDA search method have not been implemented.");
      GPSTK_THROW(e);

      // TODO: CHECK UNEXPECTED INPUT
      // n - number of float parameters
      // m - number of fixed solutions
      // L - nxn
      // D - nx1
      // zs - nxn
      // zn - nxm
      // s  - m
      const int LOOPMAX = 10000;
      const int n = L.rows();

      zn.resize(n,m,0.0);
      s.resize(m,0.0);

      Matrix<double> S(n,n,0.0);
      Vector<double> dist(n,0.0),zb(n,0.0),z(n,0.0),step(n,0.0);

      int k=n-1; dist[k]=0.0;
      zb(k)=zs(k);
      z(k)=round(zb(k)); 
      double y=zb(k)-z(k); 
      step(k)=sign(y);

      int c(0),nn(0),imax(0);
      double maxdist=1E99;
      for(int c=0;c<LOOPMAX;c++)
      {
         double newdist=dist(k)+y*y/D(k);
         if(newdist<maxdist) 
         {
            if(k!=0) 
            {
               dist(--k)=newdist;
               for(int i=0;i<=k;i++)
               {
                  S(k,i)=S(k+1,i)+(z(k+1)-zb(k+1))*L(k+1,i);
               }
               zb(k)=zs(k)+S(k,k);
               z(k)=round(zb(k)); y=zb(k)-z(k); step(k)=sign(y);
            }
            else 
            {
               if(nn<m) 
               {
                  if(nn==0||newdist>s(imax)) imax=nn;
                  for(int i=0;i<n;i++) zn(i,nn)=z(i);
                  s(nn++)=newdist;
               }
               else 
               {
                  if(newdist<s(imax)) 
                  {
                     for(int i=0;i<n;i++) zn(i,imax)=z(i);
                     s(imax)=newdist;
                     for(int i=imax=0;i<m;i++) if (s(imax)<s(i)) imax=i;
                  }
                  maxdist=s(imax);
               }
               z(0)+=step(0); y=zb(0)-z(0); step(0)=-step(0)-sign(step(0));
            }
         }
         else 
         {
            if(k==n-1) break;
            else 
            {
               k++;
               z(k)+=step(k); y=zb(k)-z(k); step(k)=-step(k)-sign(step(k));
            }
         }
      }
      for(int i=0;i<m-1;i++) 
      { 
         for(int j=i+1;j<m;j++) 
         {
            if(s(i)<s(j)) continue;
            swap(s(i),s(j));
            for(k=0;k<n;k++) swap(zn(k,i),zn(k,j));
         }
      }

      if (c>=LOOPMAX) 
      {
         return -1;
      }

      return 0;

   }  // End of method 'ARLambda::search()'


   int ARLambda::lambda( const Vector<double>& a, 
                         const Matrix<double>& Q, 
                         Matrix<double>& F,
                         Vector<double>& s,
                         const int& m )
   {
      if( (a.size()!=Q.rows()) || (Q.rows()!=Q.cols()) ) return -1;
      if( m < 1) return -1;

      const int n = static_cast<int>(a.size());

      Matrix<double> L(n,n,0.0),E(n,m,0.0);
      Vector<double> D(n,0.0),z(n,0.0);
      Matrix<double> Z = ident<double>(n);

      if (factorize(Q,L,D)==0) 
      {      
         reduction(L,D,Z);
         z = transpose(Z)*a;

         if (search(L,D,z,E,s,m)==0) 
         {
            try
            {  // F=Z'\E - Z nxn  E nxm F nxm
               F = transpose( inverseLUD(Z) ) * E;
            }
            catch(...) 
            { return -1; }
         }
      }

      return 0;

   }  // End of method 'ARLambda::lambda()'


}   // End of namespace gpstk

---