RTKLIB专题学习(二)
今天我们来进一步学习RTKLIB中矩阵的各种运算
rtkcmn.c : rtklib common functions
1.这是A、B两矩阵的乘法运算
/* multiply matrix -----------------------------------------------------------*/
extern void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C)
{
double d;
int i,j,x,f=tr[0]=='N'?(tr[1]=='N'?1:2):(tr[1]=='N'?3:4);
for (i=0;i<n;i++) for (j=0;j<k;j++) {
d=0.0;
switch (f) {
case 1: for (x=0;x<m;x++) d+=A[i+x*n]*B[x+j*m]; break;
case 2: for (x=0;x<m;x++) d+=A[i+x*n]*B[j+x*k]; break;
case 3: for (x=0;x<m;x++) d+=A[x+i*m]*B[x+j*m]; break;
case 4: for (x=0;x<m;x++) d+=A[x+i*m]*B[j+x*k]; break;
}
if (beta==0.0) C[i+j*n]=alpha*d; else C[i+j*n]=alpha*d+beta*C[i+j*n];
}
}
2.这是矩阵的LU分解
/* LU decomposition ----------------------------------------------------------*/
static int ludcmp(double *A, int n, int *indx, double *d)
{
double big,s,tmp,*vv=mat(n,1);
int i,imax=0,j,k;
*d=1.0;
for (i=0;i<n;i++) {
big=0.0; for (j=0;j<n;j++) if ((tmp=fabs(A[i+j*n]))>big) big=tmp;
if (big>0.0) vv[i]=1.0/big; else {free(vv); return -1;}
}
for (j=0;j<n;j++) {
for (i=0;i<j;i++) {
s=A[i+j*n]; for (k=0;k<i;k++) s-=A[i+k*n]*A[k+j*n]; A[i+j*n]=s;
}
big=0.0;
for (i=j;i<n;i++) {
s=A[i+j*n]; for (k=0;k<j;k++) s-=A[i+k*n]*A[k+j*n]; A[i+j*n]=s;
if ((tmp=vv[i]*fabs(s))>=big) {big=tmp; imax=i;}
}
if (j!=imax) {
for (k=0;k<n;k++) {
tmp=A[imax+k*n]; A[imax+k*n]=A[j+k*n]; A[j+k*n]=tmp;
}
*d=-(*d); vv[imax]=vv[j];
}
indx[j]=imax;
if (A[j+j*n]==0.0) {free(vv); return -1;}
if (j!=n-1) {
tmp=1.0/A[j+j*n]; for (i=j+1;i<n;i++) A[i+j*n]*=tmp;
}
}
free(vv);
return 0;
}
3.这是LU回代
/* LU back-substitution ------------------------------------------------------*/
static void lubksb(const double *A, int n, const int *indx, double *b)
{
double s;
int i,ii=-1,ip,j;
for (i=0;i<n;i++) {
ip=indx[i]; s=b[ip]; b[ip]=b[i];
if (ii>=0) for (j=ii;j<i;j++) s-=A[i+j*n]*b[j]; else if (s) ii=i;
b[i]=s;
}
for (i=n-1;i>=0;i--) {
s=b[i]; for (j=i+1;j<n;j++) s-=A[i+j*n]*b[j]; b[i]=s/A[i+i*n];
}
}
4.这是求矩阵的逆
/* inverse of matrix ---------------------------------------------------------*/
extern int matinv(double *A, int n)
{
double d,*B;
int i,j,*indx;
indx=imat(n,1); B=mat(n,n); matcpy(B,A,n,n);
if (ludcmp(B,n,indx,&d)) {free(indx); free(B); return -1;}
for (j=0;j<n;j++) {
for (i=0;i<n;i++) A[i+j*n]=0.0;
A[j+j*n]=1.0;
lubksb(B,n,indx,A+j*n);
}
free(indx); free(B);
return 0;
}
5.这是解线性方程
/* solve linear equation -----------------------------------------------------*/
extern int solve(const char *tr, const double *A, const double *Y, int n,
int m, double *X)
{
double *B=mat(n,n);
int info;
matcpy(B,A,n,n);
if (!(info=matinv(B,n))) matmul(tr[0]=='N'?"NN":"TN",n,m,n,1.0,B,Y,0.0,X);
free(B);
return info;
}