?? ambifix.c
字號(hào):
ambifix.c
void ambifix(fp,a,x,m0,n,m,idN0,N0t,N0t1,N0_n,N_fixed)
FILE *fp;
double a[Nlow],x[N],m0,N_fixed[N];
int n,m;
long idN0[Iunknown],N0t[Iunknown],N0t1[Iunknown],N0_n[Iunknown];
{
int r,i,j,l,l1,k1,ichol,igaussj,j1,j2,Ifix;
long k,J,Js,Ns;
char *str;
double sum,f1,f2,f3,f4,f5,a11[Nlow],a21[Rlow],a22[Nlow],p[N],y[N],W[N];
double qc[Nlow],vtpv_old,vtpv_new,N_W[N],K[N],y_new[N],m01,m02,Nk[N];
int Nis[N],NI[N*8],Ni[N];
double Nk0[N],Nk2[N];
/* 為控制的輸出 */
/* 計(jì)算模糊度參數(shù) */
for(r=0,i=0;i<n;i++) if(idN0[i]>1000000) r+=1;
for(i=0;i<n;i++) p[i]=0.;
/* 設(shè)置 W, y 向量 */
for(i=0;i<n-r;i++) y[i]=x[i];
for(i=0;i<r;i++) W[i]=x[i+n-r];
/* 設(shè)置 Q22 */
for (i=0;i<r;i++) {
for (j=0;j<=i;j++) {
a22[i*(i+1)/2+j]=a[(i+n-r)*((i+n-r)+1)/2+(j+n-r)];
a11[i*(i+1)/2+j]=a22[i*(i+1)/2+j]; }}
/* 矩陣 Q22 求逆 */
switch(Inv_n)
{ case 1: if (chol_inv(fp,a22,p,r)==1)
{ fprintf(fp,"Function chol_inv failed\n");
ichol = 1; }
break; }
/* 單位矩陣試驗(yàn)： E = Inv(a)*a */
for (i=0;i<n-r;i++) /* 設(shè)置Q11 */
{ for (j=0;j<=i;j++)
{ a11[i*(i+1)/2+j]=a[i*(i+1)/2+j]; }}
for (i=n-r;i<n;i++) /* 設(shè)置 Q21 */
{ for (j=0;j<n-r;j++)
{ a21[(i-(n-r))*(n-r)+j]=a[i*(i+1)/2+j]; }}
/* 計(jì)算 Qc = Q11-Q12*Inv(Q22)*Q21 */
for (i=0;i<n-r;i++)
{ for (j=0;j<=i;j++)
{ sum = 0.;
for (l=0;l<r;l++)
{ f1 = a21[l*(n-r)+i];
for (k=0;k<r;k++)
{ f3 = a21[k*(n-r)+j];
if (l>=k) f5 = a22[l*(l+1)/2+k];
else f5 = a22[k*(k+1)/2+l];
sum += f1*f3*f5; }}
qc[i*(i+1)/2+j] = a11[i*(i+1)/2+j] - sum; }}
/* 將 sqrt(qc[i][i]) 放入 p[i] */
for(i=0;i<n-r;i++) p[i]=sqrt(qc[i*(i+1)/2+i]);
vtpv_old = m0*m0*(m-n);
m01 = 9999.; m02 = m01; j1 = 0; j2 = j1;
/* 將向量 N 分組并 Nk初始化 */
NisNk(n,r,idN0,N0t,N0t1,N0_n,W,Nis,&Ns,Nk);
/* 計(jì)算所有可能的組合 */
Ifix=Ambiguity;
NINi(r,W,Ifix,Ni,NI,&Js);
fprintf(fp,"cpn,Ns=%d %d\n\n",Js,Ns);
fprintf(fp,"search areas:\n");
for(i=0;i<r;i++)
{ for(j=0;j<Ni[i];j++) fprintf(fp,"%d ",NI[j+i*8]);
fprintf(fp,"\n"); }
fprintf(fp,"\n");
/* 計(jì)算參數(shù)向量分組后的全部可能組合 */
Js=0;
for(k=1;k<=Ns;k++)
{ for(J=1,i=0;i<r;i++) if(Nis[i]==k) J*=Ni[i];
Js+=J; } /* 計(jì)算本組的可能數(shù) */
for(k=1;k<=Ns;k++) /* 計(jì)算每一組 */
{ for(Js=1,i=0;i<r;i++) if(Nis[i]==k) Js*=Ni[i];
for(J=0;J<Js;J++) /* Js 是第k組搜索可能總數(shù)， J 是指標(biāo) */
{ getNk(J,r,Nk,NI,Ni,k,Nis,Js); /*根據(jù)J求 Nk */
endN:;
for(i=0;i<r;i++) N_W[i]=W[i]-Nk[i]; /* 由于 12)., N-W 應(yīng)為 W-N */
for(i=0;i<r;i++)
{ K[i]=0.; /*向量： K = Inv(Q22)*(N - W) */
for(j=0;j<r;j++)
{ if(i>=j) K[i]+=a22[i*(i+1)/2+j]*N_W[j];
else K[i]+=a22[j*(j+1)/2+i]*N_W[j]; }}
/* (N_W)T*K, VTPV_new */
f2 = 0.;
for(i=0;i<r;i++) f2 += N_W[i]*K[i];
if(f2<0.) {f2=0.; printf("f2 = %8.4f ??????? \n",f2);}
vtpv_new = vtpv_old + f2;
sum = sqrt(vtpv_new/(m-n+r)); /* sum 是新值 m0 */
if(k==Ns&&J==0) { m01 = 9999.; m02 = m01;}
if(k==Ns && J==Js); else
min2m0(sum,&m01,&m02,Nk,Nk0,Nk2,r);
/* 求新值： Y_new = Y - Q12*K = Y - Q12[i][j]*K[j] = Y - Q21[j][i]*K[j]*/
for(i=0;i<n-r;i++) { f1=0.;
for(j=0;j<r;j++) f1 += a21[j*(n-r)+i]*K[j];
y_new[i] = -f1; }
for(i=0;i<n-r;i++) y_new[i] += y[i];
/* 如果 J==Js-1 則試驗(yàn)ratio= m02/m01 */
if(k==Ns && J==Js-1)
{ J+=1; /*fprintf(fp,"Js = %d\n",Js);*/
for(i=0;i<r;i++) {Nk[i]=Nk0[i]; N_fixed[i+n-r]=Nk0[i];}
goto endN; }
if(k==Ns && J==Js) {M0best=m01;Ratio=m02/m01;
for(i=0;i<n-r;i++) { N_fixed[i]=y_new[i];}}
/* 如果 J==Js 則試驗(yàn)恰好固定的結(jié)果 */
if(k==Ns && J==Js)
{ J+=1;
for(i=0;i<r;i++) Nk[i]=(double)((int)dfix(W[i]));
goto endN; }
} /* J 循環(huán) */
} /* k 循環(huán)，只有一組正確 */
/* 輸出最后結(jié)果 */
if(Ratio<2.) printf("KSGSoft fix Ratio = %6.4f < 2.\n",Ratio);
else printf("KSGSoft fix Ratio = %6.4f >= 2.\n",Ratio);
printf("KSGSoft best m0 = %6.4f\n",M0best);
printf("KSGSoft best ambiguity fixing:\n");
for(i=0;i<r;i++) printf("%3.0f ",N_fixed[i+n-r]);
printf("\n");
printf("related coord. solution:\n");
for(i=0;i<n-r;i++) printf("%9.4f ",N_fixed[i]);
printf("\n");}
void min2m0(m0,m01,m02,Nk,Nk0,Nk2,r)
/* 函數(shù) min2 記憶兩個(gè)最小值 m01, m02 及其相關(guān)向量 Nk0[].*/
double m0,*m01,*m02,Nk[N],Nk0[N],Nk2[N];
int r;
{ int i;
if(m0<*m01) {*m02=*m01; *m01=m0;
for(i=0;i<r;i++) Nk0[i]=Nk[i]; }
else
{ if(m0<*m02) {*m02=m0;
for(i=0;i<r;i++) Nk2[i]=Nk[i]; } } }
void NisNk(n,r,idN0,N0t,N0t1,N0_n,W,Nis,Ns,Nk)
int n,r,idN0[N],N0t[N],N0t1[N],N0_n[N],Nis[N],*Ns;
double Nk[N],W[N];
{ int i,j,k,ns;
for(i=0;i<r;i++) Nk[i]=W[i]; /* 第一步，將全部參量放入一組 */
for(i=0;i<r;i++) Nis[i]=1; ns = 1; /* 第i組模糊度參量作為第i組 */
j = idN0[n-r]/10000 - 100; /* 取N的第一個(gè) id */
for(i=0;i<r;i++)
{ k = idN0[i+n-r]/10000 - 100;
Nis[i]+=k-j; if(Nis[i]>ns) ns = Nis[i]; }
*Ns = ns;}
void NINi(r,W,ifix,Ni,NI,cpn)
/* 此處應(yīng)用r維的浮動(dòng)模糊度解 W 以及控制整數(shù) ifix (=1,2,3,4) 計(jì)算搜索的整數(shù)模糊度可能個(gè)數(shù)Ni[i]及其相關(guān)值 NI[j+i*8]、 (j=0--N[i]-1)、 可能組合總數(shù) cpn = Ni[0]*Ni[1]*...*Ni[r-1].*/
int r,ifix,NI[N*8],Ni[N],*cpn;
double W[N];
{ double f02=0.25,f1;
int i,j,j1=1;
for(i=0;i<r;i++) /*對(duì)每一個(gè) W[i]*/
{ f1 = W[i]-(int)dfix(W[i]); /*設(shè)置set Wi[i]=f2 */
if(fabs(f1)<f02)
{ Ni[i] = ifix*2 - 1;
if(Ni[i]==1) NI[0+i*8]=0;
if(Ni[i]==3) NI[0+i*8]=-1;
if(Ni[i]==5) NI[0+i*8]=-2;
if(Ni[i]==7) NI[0+i*8]=-3;
for(j=1;j<Ni[i];j++) NI[j+i*8]=NI[j-1+i*8]+1; }
else
{ Ni[i] = ifix*2;
if(Ni[i]==2) NI[0+i*8]=0;
if(Ni[i]==4) NI[0+i*8]=-1;
if(Ni[i]==6) NI[0+i*8]=-2;
if(Ni[i]==8) NI[0+i*8]=-3;
for(j=1;j<Ni[i];j++) NI[j+i*8]=NI[j-1+i*8]+1;
if(f1<0.) for(j=0;j<Ni[i];j++) NI[j+i*8]-=1; }
/*printf("f1=%8.2f %d\n",f1,Ni[i]);*/
for(j=0;j<Ni[i];j++) {NI[j+i*8]+=(int)dfix(W[i]);
/*printf("%d ",NI[j+i*8]);*/}
/*printf("\n");*/
j1*=Ni[i]; }
*cpn=j1; }
void getNk(j,r,Nk,NI,Ni,k,Nis,cpn)
/*根據(jù) j,r,k,NI,Ni,Nis,cpn 給出模糊度試驗(yàn)向量 Nk，計(jì)算新 m0。k 是組數(shù)， Nis[] 是組數(shù)向量，
j是可能組合指標(biāo)， 0 <= j < cpn，NI, Ni 參見 NINi， r 模糊度個(gè)數(shù) */
int j,r,k,NI[N*8],Ni[N],Nis[N],cpn;
double Nk[N];
{ int j1,cpn1,i;
j1 = j; cpn1 = cpn;
for(i=0;i<r;i++) /* 向前搜索組合 */
{ if(Nis[i]==k) {
cpn1/=Ni[i]; j=j1/cpn1; Nk[i]=NI[i*8+j];
j1=j1-cpn1*j; } }}
double dfix(d)
double d;
{ if(d>0) d+=0.5;
else d-=0.5;
return(d);
}
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -