matrixcsl.cpp

GPS 定位授时源码

#include "Matrixcsl.h"
#include "stdAfx.h"
#include "math.h"
#include "stdlib.h"
#include "fstream.h"


long double ** TwoArrayAlloc(int r,int c)
{
	long double  *x, **y;
	int n;

	x=(long double *)calloc(r*c,sizeof(long double));
	//x=new long double[r*c];
	if(x==NULL)
    {
	cerr<<"\n FAilue in memory applying.";
	exit(0);
    }

	y=(long double **)calloc(r,sizeof(long double *));
	//y=new long double*[r];
	for(int i=0;i<c*r;i++)
		x[i]=0;
	for(n=0;n<=r-1;++n)
		y[n]=&x[c*n];
	return(y);

}


void TwoArrayFree(long double **x)
{
	free(x[0]);
	free(x);
	//delete[]x[0];
	//delete[]x;
}


int  InvMatrix(int n,long double **a,long double **b)
{
	int *is,*js,i,j,k,l,u,v;
    long double d,p;
    is = (int *)malloc(n*sizeof(int));
    js = (int *)malloc(n*sizeof(int));
// 	is=new int[n];
// 	js=new int[n];

	long double *c = new long double[n*n];
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			c[i*n+j] = a[i][j];			
    // 以上部分是声明数据，将二维阵a存入一维阵c                            
    
    for (k=0; k<=n-1; k++)
	//从全行全列起，每次递减一行一列，对其选全主元
      {
        d=0.0;
        
        for (i=k; i<=n-1; i++)
        for (j=k; j<=n-1; j++)
          { l=i*n+j; p=fabs(c[l]);
            if (p>d) { d=p; is[k]=i; js[k]=j;}
          }
       //从n-k阶方阵中选取绝对值最大的元素 
        if (d+1.0==1.0)
          { free(is); free(js); 
// 			delete[]is;
// 			delete[]js;
			delete[] c;printf("     Caution:   The Matrix Can not Inv\n");
            return(0);}
          //若绝对值最大值还是为0

        if (is[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=is[k]*n+j;
              p=c[u]; c[u]=c[v]; c[v]=p;
            }
        //行对换  最大元素行is[k]换到第K行  
        
        if (js[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+js[k];
              p=c[u]; c[u]=c[v]; c[v]=p;
            }
       //列对换   最大元素列js[k]换到第k列
		  
        l=k*n+k;
	  //l为最大元素在以一维里的序号
        c[l]=1.0/c[l];
      //最大元取倒数得到最小比
        for (j=0; j<=n-1; j++)
          if (j!=k)
            { u=k*n+j; c[u]=c[u]*c[l];}
      //第K行全部除于最大元，即乘于最小比（K行K列元素前面已经处理）      
        for (i=0; i<=n-1; i++)
          if (i!=k)
            for (j=0; j<=n-1; j++)
              if (j!=k)
                { u=i*n+j;
                  c[u]=c[u]-c[i*n+k]*c[k*n+j];
                }
        //对既非K行也非K列的元素处理 （减去对应行号k列与K行列号元素的积）       
        for (i=0; i<=n-1; i++)
          if (i!=k)
            { u=i*n+k; c[u]=-c[u]*c[l];}
		 //第K列元素反号并除于最大元，即乘于最小比
    //以上三段代码最核心
	}
    
    
    //倒序换回主元，穿脱律
    for (k=n-1; k>=0; k--)//k从n-1开始到0
      { 
      if (js[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=js[k]*n+j;
              p=c[u]; c[u]=c[v]; c[v]=p;
            }
        //第k行与第js[k]行（最大元素所在列号）互换
        if (is[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+is[k];
              p=c[u]; c[u]=c[v]; c[v]=p;
            }
		}
        //第k列与第is[k]列（最大元素所在行号）互换

    free(is); free(js);
// 	delete[]is;
// 	delete[]js;
	//存储的是不同k值的最大元所在行列序号
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
			b[i][j] = c[i*n+j];
	  delete[] c;
    return(1);
}
//程世来于2006年3月1日详细注释


//    NBB=A'A  V=AX+L;
void FormNormMatrix(long double **A,int m,int n,long double **NBB)
{
	for(int tem=0;tem<n;tem++)
		for(int met=0; met<n; met++)
			*(*(NBB+tem)+met)=0;
	for(int i=0;i<n;i++)
		for(int k=0;k<n;k++)
			for(int j=0;j<m;j++)
				*(*(NBB+i)+k)=*(*(NBB+i)+k)+(*(*(A+j)+i)) * (*(*(A+j)+k));
}

//A m*l ; B  l*n ;   C  m*n;
void  AMBProduct(long double **A,int m,int l,long double **B,int n,long double **C)
{
	for(int i0=0;i0<m;i0++)
		for(int j0=0;j0<n;j0++)
			//   C[i0][j0]=0; 
			*(*(C+i0)+j0)=0;

		for(int i=0;i<m;i++)
			for(int j=0;j<n;j++)
				for(int k=0;k<l;k++)
					//C[i][j]+=A[k][i]*B[k][j];
					*(*(C+i)+j)+=*(*(A+i)+k)*(*(*(B+k)+j));
}

void AtMatrics(long double **A,int m,int l,long double **At)
{
	for(int i0=0;i0<l;i0++)
		for(int j0=0;j0<m;j0++)
			*(*(At+i0)+j0)=*(*(A+j0)+i0);
		
}