?? adjustment.cpp
字號:
if (t>d) d=t;
sm=s[mm-1]/d; sm1=s[mm-2]/d;
em1=e[mm-2]/d;
sk=s[kk-1]/d; ek=e[kk-1]/d;
b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0;
c=sm*em1; c=c*c; shh=0.0;
if ((b!=0.0)||(c!=0.0))
{ shh=sqrt(b*b+c);
if (b<0.0) shh=-shh;
shh=c/(b+shh);
}
fg[0]=(sk+sm)*(sk-sm)-shh;
fg[1]=sk*ek;
for (i=kk; i<=mm-1; i++)
{ Adjustment::sss(fg,cs);
if (i!=kk) e[i-2]=fg[0];
fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
fg[1]=cs[1]*s[i];
s[i]=cs[0]*s[i];
if ((cs[0]!=1.0)||(cs[1]!=0.0))
for (j=1; j<=n; j++)
{ ix=(j-1)*n+i-1;
iy=(j-1)*n+i;
d=cs[0]*v[ix]+cs[1]*v[iy];
v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
v[ix]=d;
}
Adjustment::sss(fg,cs);
s[i-1]=fg[0];
fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
fg[1]=cs[1]*e[i];
e[i]=cs[0]*e[i];
if (i<m)
if ((cs[0]!=1.0)||(cs[1]!=0.0))
for (j=1; j<=m; j++)
{ ix=(j-1)*m+i-1;
iy=(j-1)*m+i;
d=cs[0]*u[ix]+cs[1]*u[iy];
u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
u[ix]=d;
}
}
e[mm-2]=fg[0];
it=it-1;
}
else
{ if (ks==mm)
{ kk=kk+1;
fg[1]=e[mm-2]; e[mm-2]=0.0;
for (ll=kk; ll<=mm-1; ll++)
{ i=mm+kk-ll-1;
fg[0]=s[i-1];
Adjustment::sss(fg,cs);
s[i-1]=fg[0];
if (i!=kk)
{ fg[1]=-cs[1]*e[i-2];
e[i-2]=cs[0]*e[i-2];
}
if ((cs[0]!=1.0)||(cs[1]!=0.0))
for (j=1; j<=n; j++)
{ ix=(j-1)*n+i-1;
iy=(j-1)*n+mm-1;
d=cs[0]*v[ix]+cs[1]*v[iy];
v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
v[ix]=d;
}
}
}
else
{ kk=ks+1;
fg[1]=e[kk-2];
e[kk-2]=0.0;
for (i=kk; i<=mm; i++)
{ fg[0]=s[i-1];
Adjustment::sss(fg,cs);
s[i-1]=fg[0];
fg[1]=-cs[1]*e[i-1];
e[i-1]=cs[0]*e[i-1];
if ((cs[0]!=1.0)||(cs[1]!=0.0))
for (j=1; j<=m; j++)
{ ix=(j-1)*m+i-1;
iy=(j-1)*m+kk-2;
d=cs[0]*u[ix]+cs[1]*u[iy];
u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
u[ix]=d;
}
}
}
}
}
}
return(1);
}
/******廣意矩陣求逆*********
本函數的功能為：將矩陣a的逆矩陣放入aa中
a為m*n矩陣的地址；
aa為用于存放a逆矩陣的地址；
u為m*m矩陣的地址；
v為n*n矩陣的地址；
esp為0.00001。
*********************************/
int Adjustment::bginv(double *a,int m,int n,double *aa,double eps,double *u,double *v,int ka)
{ int i,j,k,l,t,p,q,f;
extern int bmuav();
i=Adjustment::bmuav(a,m,n,u,v,eps,ka);
if (i<0) return(-1);
j=n;
if (m<n) j=m;
j=j-1;
k=0;
while ((k<=j)&&(a[k*n+k]!=0.0)) k=k+1;
k=k-1;
for (i=0; i<=n-1; i++)
for (j=0; j<=m-1; j++)
{ t=i*m+j; aa[t]=0.0;
for (l=0; l<=k; l++)
{ f=l*n+i; p=j*m+l; q=l*n+l;
aa[t]=aa[t]+v[f]*u[p]/a[q];
}
}
return(1);
}
/*********矩陣相乘**********
本函數的功能為:將矩陣a與b相乘的結果放入c中
a為m*n矩陣的地址;
b為n*k矩陣的地址；
c為m*k矩陣的地址。
*********************************/
void Adjustment::brmul(double *a,double *b,int m,int n,int k,double *c)
{ int i,j,l,u;
for (i=0; i<=m-1; i++)
for (j=0; j<=k-1; j++)
{ u=i*k+j; c[u]=0.0;
for (l=0; l<=n-1; l++)
c[u]=c[u]+a[i*n+l]*b[l*k+j];
}
return;
}
/*********參數平差**********
本函數的功能為:將觀測方程形式為V=AX-L的平差結果放入pra矩陣中
A為m*n的系數矩陣首地址;
P為m*m的權矩陣首地址；
L為m*1的常數矩陣首地址；
pra為n*1的矩陣首地址,用于存放平差結果。
*********************************/
void Adjustment::ParameterAdjustment(double *A,double *P,double *L,int m,int n,double *Pra)
{
int i,j;
int ka=n+1;
double *P1,*P2,*P3,*P4,*P5,*P6,*P7,eps;
eps=0.00001;
P1=new double[m*n];
P2=new double[n*m];
P3=new double[n*n];
P4=new double[n];
P5=new double[n*n];
P6=new double[n*n];
P7=new double[n*n];
//矩陣轉秩，P1存放A的轉秩陣
for(i=0;i<m;i++)
for(j=0;j<n;j++)
{
P1[j*m+i]=A[i*n+j];
}
//計算法方程矩陣N即A'pA
brmul(P1,P, n, m,m,P2);
brmul(P2,A, n, m,n,P3);
//計算法方程矩陣L即A'pl
brmul(P2,L, n, m,1,P4);
//求矩陣N的逆
bginv(P3, n, n,P5,eps,P6,P7,ka);
//求平差結果
brmul(P5,P4, n, n,1,Pra);
for(i=0;i<n;i++)
{
Pra[i]=-Pra[i];
}
delete[] P1;
delete[] P2;
delete[] P3;
delete[] P4;
delete[] P5;
delete[] P6;
delete[] P7;
}
/*****************具有條件的參數平差**************************************
本函數的功能為:將觀測方程形式為V=AX-L,條件方程為：BX+w=0的平差結果放入pra矩陣中
*A參數系數, *P權陣, *L參數式中的常數項,
*B條件式系數, *w條件式常數項,
m--A的行數,n-- A、B的列數 , r--B的行數,
*Pra--存儲參數的或然值, *DitXYZ--各參數的中誤差
********************************************************************/
void Adjustment::ParameterAdjustmentWithCondi(double *A, double *P, double *L, double *B, double *w, int m, int n, int r, double *Pra, double *DitPra)
{
int i,j;
int ka=n+1;
double *P1,*P2,*P3,*P4,*P5,*P6,*P7,eps;
double *P8,*P9,*P10,*P11,*P12,*P13,*P14,*P15,*P16,*P17;
eps=0.00001;
P1=new double[m*n];
P2=new double[n*m];
P3=new double[n*n];
P4=new double[n];
P5=new double[n*n];
P6=new double[n*n];
P7=new double[n*n];
P8=new double[r*n];
P9=new double[r*n];
P10=new double[r*r];
P11=new double[n*1];
P12=new double[r*r];
P13=new double[r*r];
P14=new double[r*r];
P15=new double[r];
P16=new double[r];
P17=new double[n];
//A的轉秩
for(i=0;i<n;i++)
for(j=0;j<m;j++)
{
P1[i*m+j]=A[j*n+i];
}
//B的轉秩=P8
for(i=0;i<n;i++)
for(j=0;j<r;j++)
{
P8[i*r+j]=B[j*n+i];
}
//N=P3
brmul(P1,P,n,m,m,P2);//矩陣相乘
brmul(P2,A, n, m,n,P3);
//L=P4,即A'Pl
brmul(P2,L, n, m,1,P4);
//N的逆陣=P5
bginv(P3, n, n,P5,eps,P6,P7,ka);//矩陣求逆
//K
brmul(B,P5, r, n,n,P9);//矩陣相乘B*N-1
brmul(P9,P8, r, n,r,P10);//B*N-1*Bt=P10
bginv(P10, r, r,P12,eps,P13,P14,ka);//矩陣求逆
brmul(P9,P4, r, n,1,P15);//B*N-1*Bt=P10
for(i=0;i<r;i++)
P15[i]=w[i]-P15[i];//Bt*B*(N-1)*U+U=P4
//參數值
brmul(P12,P15, r, r,1,P16);//K
brmul(P8,P16, n, r,1,P17);//BT*k
//BT*k+U
for(i=0;i<n;i++)
{
P17[i]+=P4[i];
}
brmul(P5,P17, n, n,1,Pra);//BT*k
for(i=0;i<n;i++)
{
Pra[i]=-Pra[i];
}
delete[] P1;
delete[] P2;
delete[] P3;
delete[] P4;
delete[] P5;
delete[] P6;
delete[] P7;
delete[] P8;
delete[] P9;
delete[] P10;
delete[] P11;
delete[] P12;
delete[] P13;
delete[] P14;
delete[] P15;
delete[] P16;
delete[] P17;
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -