LAMBDA算法

lambda算法是一种基于最小二乘估计调整的搜索算法，是当前广泛应用的基于gps载波相位进行定位，测姿和定向的算法。

# include 
# include 
# include 
# include 
# include  //屏幕处理函数放在conio.h中
# include 
# include 
using namespace std;

const double pi=3.141593;
void ldl（double **Qdouble **Ldouble **Dint n）;  //对矩阵Q进行LtDL分解，Lt为下三角矩阵，D为对角矩阵
int round（double a）;  //对a四舍五入
void Inv（double **Aint n）;  //计算矩阵的逆
void MatrixMultiply（double **Adouble **B double **Cint n）;   //计算矩阵乘积
void threeMultiply（double **Adouble **Bdouble **Cint n）;
void decorrel（double **Qdouble *aint **Zdouble **Ddouble **Ldouble *zint n）;  //去相关计算
int sign（double a）;
double chistart（double **Ddouble **Ldouble *aint ncandsdouble factorint n）;
double gamma（double x）;  //计算伽马（gamma）函数值
void reform（double *Qint *bint ncands）;  //将数组Q中元素从小到大排列，并记录顺序
void lsearch（double *afloatdouble **Ldouble **Ddouble Chi2
			 double **afixeddouble *sqnormint &ierrint nint ncands）;
bool ifdynamic（double **Qahatint n）;


int main（）
{
	int n=62;
	/*cout<<“请输入协方差矩阵维数：“;
	cin>>n;*/
	int ncands=2ijkierr;
	double Chi2factor=1.5;
	double **Qahat**L**D**afixed**ahelp2**Qhelp**Zhelp;
	Qahat=new double*[n];L=new double*[n];D=new double*[n];afixed=new double*[n];
	ahelp2=new double*[n];Qhelp=new double*[n];Zhelp=new double*[n];
	
	double **ahelp;
	ahelp=new double*[ncands];
	for（i=0;i		ahelp[i]=new double[n];
	double *afloat*z*sqnorm;
	afloat=new double[n];z=new double[n];sqnorm=new double[ncands];

	i=0;
	fstream fin1（“F:\\float_B.txt“）;
	for（i=0;i		fin1>>afloat[i];

	int **Z*incr;
	Z=new int*[n];
	incr=new int[n];
	for（i=0;i	{
		Qahat[i]=new double[n];L[i]=new double[n];D[i]=new double[n];afixed[i]=new double[ncands];
		ahelp2[i]=new double[ncands];Qhelp[i]=new double[n];Zhelp[i]=new double[n];Z[i]=new int[n];
	}
	fstream fin2（“F:\\Q_BB.txt“）;
	for（i=0;i		for（j=0;j			fin2>>Qahat[i][j];

	bool TT1;
	TT1=ifdynamic（Qahatn）;
	if（!TT1）
	{
		cout<<“错误！输入的协方差阵不是对称矩阵。“<		return 0;
	}
	for（i=0;i		for（j=0;j			Qhelp[i][j]=Qahat[i][j];
	ldl（QhelpLDn）;
	for（i=0;i		if（D[i][i]<0）
		{
			cout<<“错误！输入的协方差阵不是正定矩阵。“<			return 0;
		}
	for（i=0;i		incr[i]=afloat[i];
	for（i=0;i		afloat[i]=afloat[i]-incr[i];
	decorrel（QahatafloatZDLzn）;
	Chi2=chistart（DLzncandsfactorn）;
	lsearch（zLDChi2afixedsqnormierrnncands）;
	if（ierr==1）
	{
		cout<<“没有足够的候选解！“<		return 0;
	}
	for（i=0;i		for（j=0;j			Zhelp[i][j]=Z[i][j];
	Inv（Zhelpn）;
	for（i=0;i		for（j=0;j			ahelp[j][i]=afixed[i][j];
	for（i=0;i		for（j=0;j		{
			ahelp2[j][i]=0;
			for（k=0;k				ahelp2[j][i]+=ahelp[i][k]*Zhelp[k][j];
		}
	for（j=0;j		for（i=0;i			ahelp2[i][j]+=incr[i];
	for（i=0;i		for（j=0;j			afixed[i][j]=round（ahelp2[i][j]）;
	fstream fout1（“F:\\固定解.txt

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件     630847  2012-07-02 17:25  LAMBDA\Debug\lambda2.exe

     文件     852012  2012-07-02 17:25  LAMBDA\Debug\lambda2.ilk

     文件     381452  2012-07-02 17:25  LAMBDA\Debug\lambda2.obj

     文件       5277  2012-06-29 08:53  LAMBDA\Debug\simple.obj

     文件      82944  2012-07-02 17:25  LAMBDA\Debug\vc60.idb

     文件     135168  2012-07-02 17:25  LAMBDA\Debug\vc60.pdb

     文件   17367040  2012-07-31 09:38  LAMBDA\ipch\lambda2-b958b990\lambda2-6ea0b709.ipch

     文件      22861  2012-04-12 21:44  LAMBDA\LAMBDA C编程.txt

     文件      18810  2012-07-02 17:24  LAMBDA\lambda2.cpp

     文件       3413  2012-07-02 17:25  LAMBDA\lambda2.dsp

     文件        537  2012-07-02 17:30  LAMBDA\lambda2.dsw

     文件      41984  2012-07-02 17:30  LAMBDA\lambda2.ncb

     文件      48640  2012-07-02 17:30  LAMBDA\lambda2.opt

     文件        749  2012-07-02 17:25  LAMBDA\lambda2.plg

     文件    5918720  2012-07-31 09:45  LAMBDA\lambda2.sdf

     文件       2560  2012-07-31 09:43  LAMBDA\lambda2.suo

     文件       6120  2012-07-31 09:36  LAMBDA\lambda2.vcxproj

     文件        143  2012-07-31 09:36  LAMBDA\lambda2.vcxproj.user

     目录          0  2012-08-07 10:23  LAMBDA\ipch\lambda2-b958b990

     目录          0  2012-08-07 10:23  LAMBDA\Debug

     目录          0  2012-08-07 10:23  LAMBDA\ipch

     目录          0  2012-08-07 10:23  LAMBDA

----------- ---------  ---------- -----  ----

             25519277                    22