A(nxm)=A[i+j*n]=A((i+1)(j+1))
mat:在矩阵算法上使用的是mxn。
extern double *mat(int n, int m)
{
double *p;
if (n<=0||m<=0) return NULL;
if (!(p=(double *)malloc(sizeof(double)*n*m))) {
fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);
}
return p;
}
norm:||a||。 a(nx1)
extern double norm(const double *a, int n)
{
return sqrt(dot(a,a,n));
}
matmul:tr是N和T的组合,N:原矩阵。T:转置。
C=alpha (AB) (N or T) + beta C 。
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];
}
}
initx:协方差替换,对P的替换,对角i,例如i=2时,替换2x2位置为var。。浮点状态x
static void initx(rtk_t *rtk, double xi, double var, int i)
{
int j;
rtk->x[i]=xi; //
for (j=0;j<rtk->nx;j++) {
rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0; // i是否等于j,如果是,把var赋值给P
}
}
matinv:矩阵的逆。
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;
}
dot:矩阵内积,需要注意为nx1。
c=a'*b。
extern double dot(const double *a, const double *b, int n)
{
double c=0.0;
while (--n>=0) c+=a[n]*b[n];
return c;
}