lambda.cpp

GPS 定位授时源码

			}
			if (nTemp != i)
			{
				dSortMid			= Chi.pMATRIX[0][i];
				Chi.pMATRIX[0][i]	= Chi.pMATRIX[0][nTemp];
				Chi.pMATRIX[0][nTemp]	= dSortMid;
			}
		}
		

		dChi2	= Chi.pMATRIX[0][nCands - 1] + 1e-6;
	}
	else//要求的候选整周模糊度组合大于整周模糊度的个数加1时才能用到
	{//还没有做//有问题，不知道源Matlab程序中gamma()函数怎么实现
		Linv	=  ~L;
		Dinv	=  ~D;
		double   dVn;
		int      nNH;
		dVn		= 1;
		for(nNH = int(nN / 2 - 1);nNH > 0; nNH --)
		{
			dVn  = dVn * nNH; 
		}
		dVn		=  (2.0 / nN) * sqrt(pow(4*atan(1.0),nN)) / dVn;
		double   dProduct;
		dProduct		= 1;
		for(int i = 0;i <D.nRow;i ++)
		{
			dProduct *= D.pMATRIX[i][i];
		}
		dChi2	= 1.5 * (nCands / sqrt(dProduct * dVn));
	}

	return dChi2;
	
}
// ***************************************************************************


// ***************************************************************************
void lSearch(MATRIX Afloat,MATRIX L,MATRIX D, double dChi2, int nCands,MATRIX &AfixedOut,MATRIX &SQahatnormOut)
{
	MATRIX  Linv;
	MATRIX	Dinv;

	Linv	= ~L;
	Dinv	= ~D;
	
	int			nN;
	if (Afloat.nRow > Afloat.nCol)
	{
		nN		= Afloat.nRow;
	}
	else
	{
		nN		= Afloat.nCol;
	}
	MATRIX	Right(nN + 1,1);

	Right.pMATRIX[nN][0] = dChi2;

	MATRIX	Left(nN + 1,1);

	MATRIX	DQahat(1,nN);
	MATRIX dist(nN,1);
	MATRIX Lef(1,nN);
	MATRIX endd(nN,1);
	
	for(int i= 1,j = 0;i < nN; i ++,j ++)
	{
		DQahat.pMATRIX[0][j]	= Dinv.pMATRIX[i][i] / Dinv.pMATRIX[i - 1][i - 1];
	}
	DQahat.pMATRIX[0][j]		= 1.0 / Dinv.pMATRIX[nN - 1][nN - 1];

	int		nCan		= 0;
	int		ni			= nN + 1;
	int		niOld		= ni;
	int		niErr		= 0;

	MATRIX	Afixed(nN,nCands);
	MATRIX  SQahatnorm(1,nCands);
	bool	bEndSearch	= false;
	bool	bC_Stop		= false;
	bool	bCand_n		= false;

	while (!bEndSearch)
	{
		ni			= ni - 1;
//		MATRIX		Lef(1,ni);
		if (niOld <=  ni )
		{
			Lef.pMATRIX[0][ni - 1]	= Lef.pMATRIX[0][ni - 1] + Linv.pMATRIX[ni][ni - 1];
		}
		else
		{
			Lef.pMATRIX[0][ni - 1]	= 0;
			for(j = ni;j < nN; j ++)
			{
				Lef.pMATRIX[0][ni - 1] = Lef.pMATRIX[0][ni - 1] + Linv.pMATRIX[j][ni - 1] * dist.pMATRIX[j][0];
			}
		}

		niOld		= ni;
		Right.pMATRIX[ni - 1][0]   = (Right.pMATRIX[ni][0] - Left.pMATRIX[ni][0]) * DQahat.pMATRIX[0][ni - 1];
		double  dReach;
		dReach		= sqrt(Right.pMATRIX[ni - 1][0]);
		double   dDelta;
		dDelta		= Afloat.pMATRIX[ni - 1][0]  - dReach - Lef.pMATRIX[0][ni - 1];
		double		dIntDDelta;
		if (dDelta >= 0)
		{//正整数
			if ((dDelta - int(dDelta))> 0) 
			{
				dIntDDelta  = int(dDelta + 1);
			}
			else
			{
				dIntDDelta	= dDelta;
			}
		}
		else
		{
			dIntDDelta	= int(dDelta);
		}
		dist.pMATRIX[ni-1][0]		= dIntDDelta  - Afloat.pMATRIX[ni - 1][0]; 

		if(dist.pMATRIX[ni - 1][0] > (dReach - Lef.pMATRIX[0][ni - 1]))
		{
			bCand_n			= false;
			bC_Stop			= false;
			while (!bC_Stop &&  ni < nN )//- 1做了改动
			{
//				ni = ni + 1;
				if (dist.pMATRIX[ni][0] < endd.pMATRIX[ni][0])
				{
					dist.pMATRIX[ni][0]  = dist.pMATRIX[ni][0] + 1;
					Left.pMATRIX[ni][0]	 = pow((dist.pMATRIX[ni][0] + Lef.pMATRIX[0][ni]),2);
					bC_Stop			= true;
					ni = ni + 1;
					if (ni == (nN ))//- 1
					{
						bCand_n			= true;
					}
				}
				else
				{
					ni  = ni + 1;
				}
			}
			if (ni == (nN ) && (!bCand_n))//-1
			{
				bEndSearch		= true;
			}
		}
		else
		{
			endd.pMATRIX[ni - 1][0]		= dReach  - Lef.pMATRIX[0][ni - 1] - 1;
			Left.pMATRIX[ni - 1][0]		= pow(dist.pMATRIX[ni - 1][0] + Lef.pMATRIX[0][ni - 1],2);
		}
		

		if ((ni - 1) == 0)//改动过
		{
			double t;
			t	= dChi2 - (Right.pMATRIX[0][0] - Left.pMATRIX[0][0]) * Dinv.pMATRIX[0][0];
			endd.pMATRIX[0][0]	= endd.pMATRIX[0][0]	+ 1;

			while (dist.pMATRIX[0][0]  <= endd.pMATRIX[0][0])
			{
				if (nCan < nCands)
				{
					nCan		= nCan + 1;
					for(int aFloat = 0;aFloat < Afixed.nRow;aFloat ++)
					{
						Afixed.pMATRIX[aFloat][nCan - 1]  = dist.pMATRIX[aFloat][0] + Afloat.pMATRIX[aFloat][0];

					}
					SQahatnorm.pMATRIX[0][nCan - 1]		= t;
				
				}
				else
				{
					double dMaxNorm;
					int		nPos;
					dMaxNorm	= SQahatnorm.pMATRIX[0][0];
					nPos        = 0;
					for(int u=0;u < SQahatnorm.nRow;u ++)
					{
						if (SQahatnorm.pMATRIX[u][u] > dMaxNorm)
						{
							dMaxNorm	= SQahatnorm.pMATRIX[u][u];
							nPos		= u;
						}
					
					}
					if (t < dMaxNorm)
					{
						for(int aFloat = 0;aFloat < Afixed.nRow;aFloat ++)
						{
							Afixed.pMATRIX[aFloat][nPos]  = dist.pMATRIX[aFloat][0] + Afloat.pMATRIX[aFloat][0];
						}
						SQahatnorm.pMATRIX[nPos][nPos]		= t;
					}
				}
				t		= t + (2 * (dist.pMATRIX[0][0] + Lef.pMATRIX[0][0]) + 1) * Dinv.pMATRIX[0][0];
				dist.pMATRIX[0][0]  = dist.pMATRIX[0][0]  + 1;
			}

			bCand_n		= false;
			bC_Stop		= false;

			while (!bC_Stop && (ni < (nN)))
			{
				ni		= ni + 1;

				if (dist.pMATRIX[ni - 1][0]  < endd.pMATRIX[ni - 1][0])
				{
					dist.pMATRIX[ni - 1][0]		= dist.pMATRIX[ni - 1][0] + 1;
					Left.pMATRIX[ni - 1][0]		= pow(dist.pMATRIX[ni - 1][0] + Lef.pMATRIX[0][ni - 1],2);
					bC_Stop			= true;
					if (ni == (nN ))//-1
					{
						bCand_n		= true;

					}
				}
			}
			if ((ni == (nN)) && (!bCand_n))
			{
				bEndSearch  = true;
			}
		}
	}


	//排序
	MATRIX  Tmp(SQahatnorm.nCol,Afixed.nRow + SQahatnorm.nRow);
	for(i = 0;i < SQahatnorm.nRow;i ++)
	{
		for(j = 0 ;j < SQahatnorm.nCol;j ++)
		{
			Tmp.pMATRIX[j][i]  = SQahatnorm.pMATRIX[i][j];
		}
	}
	for(i = SQahatnorm.nRow;i < Tmp.nCol;i ++)
	{
		for(j = 0;j < Tmp.nRow;j ++)
		{
			Tmp.pMATRIX[j][i]  = Afixed.pMATRIX[i - SQahatnorm.nRow][j];
		}
	}

	int			nColPos;
	double		dTempMid;
	for(j = 0; j < Tmp.nRow;j ++)
	{
		nColPos		= j;
		for(int aFloat = j + 1;aFloat < Tmp.nRow;aFloat ++)
		{
			//每列排序
			if (Tmp.pMATRIX[aFloat][0] < (Tmp.pMATRIX[nColPos][0]))
			{
				nColPos			= aFloat;
			}
		}
		if (nColPos != j)
		{//交换整行
			for(i = 0 ;i < Tmp.nCol;i ++ )
			{
				dTempMid	= Tmp.pMATRIX[j][i];
				Tmp.pMATRIX[j][i]		= Tmp.pMATRIX[nColPos][i];
				Tmp.pMATRIX[nColPos][i]		= dTempMid;
			}
			
		}
	}
	

	for(i = 0;i < SQahatnorm.nRow;i ++)
	{
		for(j = 0;j < SQahatnorm.nCol;j ++)
		{
			SQahatnorm.pMATRIX[i][j]  = Tmp.pMATRIX[j][i];
		}

	}

	for(i = 0;i < Tmp.nRow;i ++)
	{
		for(j = SQahatnorm.nRow;j < Tmp.nCol; j ++)
		{
			Afixed.pMATRIX[j - SQahatnorm.nRow][i ] = Tmp.pMATRIX[i][j];
		}
	}
	
	AfixedOut		=  Afixed;
	SQahatnormOut	=  SQahatnorm;
	
}


int LtDLComposition(MATRIX Qahat,MATRIX & L,MATRIX & D)   //Qahat=Lt*D*L 
{
	
	D.MallocSpace(Qahat.nRow,Qahat.nCol);
	
	L.MallocSpace(Qahat.nRow,Qahat.nCol);

	int i,j,k;
	int iRow=Qahat.nRow;

	for( i=iRow;i>=1;i--)
	{
		D.pMATRIX[i-1][i-1]=Qahat.pMATRIX[i-1][i-1];
		for( j=1;j<=i;j++)
		{
			L.pMATRIX[i-1][j-1]=Qahat.pMATRIX[i-1][j-1]/sqrt(Qahat.pMATRIX[i-1][i-1]);
		}
		for( k=1;k<=i-1;k++)
		{
			for(j=1;j<=k;j++)
			{
				Qahat.pMATRIX[k-1][j-1]=Qahat.pMATRIX[k-1][j-1]-L.pMATRIX[i-1][j-1]*L.pMATRIX[i-1][k-1];
			}	
		}
		for(j=1;j<=i;j++)
		{
			L.pMATRIX[i-1][j-1]=L.pMATRIX[i-1][j-1]/L.pMATRIX[i-1][i-1];
		}
		
	}	
	return 1;
}

MATRIX RepmatMatrix(MATRIX Incur, int nRow, int nCol)
{
	MATRIX  ResultM(Incur.nRow * nRow,Incur.nCol * nCol);
	for(int i=0;i < ResultM.nRow;i ++)
	{
		for(int j = 0;j < ResultM.nCol; j ++)
		{
			ResultM.pMATRIX[i][j] = Incur.pMATRIX[i][j % Incur.nCol];
		}
	}
	return ResultM;
}