ARMLambda.cpp

Go to the documentation of this file.

#pragma ident "$Id: ARMLambda.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 "ARMLambda.hpp"


namespace gpstk
{

   int ARMLambda::search( const Matrix<double>& L, 
                          const Vector<double>& D, 
                          const Vector<double>& zs, 
                          Matrix<double>& zn, 
                          Vector<double>& s, 
                          const int& m )
   {
      // 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 'ARMLambda::search()'
   

}   // End of namespace gpstk

---