RTKLIB软件源码学习（Kalman滤波-矩阵先导）

本文是学习记录关于rib源码中使用的kalman滤波，因为整个定位系统存在误差以及不确定性，需要使用kalman滤波进行预测和平滑，在rtklib中使用的是EKF，即扩展kalman滤波，具体关于kalman滤波理论的学习参考这里，同样，本文仅解读代码部分。

首先了解定义函数部分，由于部分定义函数仅适用于矩阵方面，因此将这部分定义函数的解读放在kalman滤波这里。

1、简单矩阵

1.1、mat()

1.2、imat()

1.3、 zero()

1.4、eye()

1.5、dot()

1.6、norm()

1.7、 matcopy()

2、进阶矩阵

2.1、matmul()

2.2、ludcmp()

2.3、lubksb()

2.4、matinv()

2.5、 solve()

1、简单矩阵

1.1、mat()

创建一个n*m的矩阵

/* new matrix ------------------------------------------------------------------
* allocate memory of matrix
* args   : int    n,m       I   number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *mat(int n, int m)
{double *p;if (n<=0||m<=0) return NULL;//首先判定n和m是否大于0if (!(p=(double *)malloc(sizeof(double)*n*m))) {//这里是为一个n行m列的矩阵开辟内存空间，并同时进行是否开辟正常的判定fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);}return p;
}

1.2、imat()

创造一个n*m的整数矩阵，int类型

/* new integer matrix ----------------------------------------------------------
* allocate memory of integer matrix
* args   : int    n,m       I   number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern int *imat(int n, int m)
{int *p;if (n<=0||m<=0) return NULL;if (!(p=(int *)malloc(sizeof(int)*n*m))) {fatalerr("integer matrix memory allocation error: n=%d,m=%d\n",n,m);}return p;
}

1.3、 zero()

创造一个n*m的0矩阵

/* zero matrix -----------------------------------------------------------------
* generate new zero matrix
* args   : int    n,m       I   number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *zeros(int n, int m)
{double *p;#if NOCALLOCif ((p=mat(n,m))) for (n=n*m-1;n>=0;n--) p[n]=0.0;
#else//首先创造一个n*m矩阵，然后通过循环将矩阵的每一个元素的值减到0if (n<=0||m<=0) return NULL;if (!(p=(double *)calloc(sizeof(double),n*m))) {fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);}
#endif//不清楚这部分为什么多个定义return p;
}

1.4、eye()

创造一个n*n的主对角线元素为1的矩阵

/* identity matrix -------------------------------------------------------------
* generate new identity matrix
* args   : int    n         I   number of rows and columns of matrix
* return : matrix pointer (if n<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *eye(int n)
{double *p;int i;if ((p=zeros(n,n))) for (i=0;i<n;i++) p[i+i*n]=1.0;//这里就是在0矩阵的基础上将主对角线元素的值加一return p;
}

1.5、dot()

求两个向量的内积

/* inner product ---------------------------------------------------------------
* inner product of vectors
* args   : double *a,*b     I   vector a,b (n x 1)
*          int    n         I   size of vector a,b
* return : 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];//这里不用for循环，从后往前乘，可能是依据存放顺序的逻辑进行return c;
}

1.6、norm()

求向量的欧几里得范数，即l2范数，就是通常意义上的模

/* euclid norm -----------------------------------------------------------------
* euclid norm of vector
* args   : double *a        I   vector a (n x 1)
*          int    n         I   size of vector a
* return : || a ||
*-----------------------------------------------------------------------------*/
extern double norm(const double *a, int n)
{return sqrt(dot(a,a,n));//sqrt为返回一个数的平方根，首先求向量自己的内积再开方即为欧式范数
}

1.7、 matcopy()

将B矩阵的所有元素赋值给A矩阵

/* copy matrix -----------------------------------------------------------------
* copy matrix
* args   : double *A        O   destination matrix A (n x m)
*          double *B        I   source matrix B (n x m)
*          int    n,m       I   number of rows and columns of matrix
* return : none
*-----------------------------------------------------------------------------*/
extern void matcpy(double *A, const double *B, int n, int m)
{memcpy(A,B,sizeof(double)*n*m);//调用C库函数memcpy，与strcpy相比，memcpy并不是遇到'\0'就结束，而是一定会拷贝完n个字节//为什么要多写一个方法呢，可能是为了传参简单
}

2、进阶矩阵

下面矩阵代码中有些函数同时有两种表示，上部分 /* with LAPACK/BLAS or MKL */，下部分 /* without LAPACK/BLAS or MKL */，现在大多数函数库都是基于BLAS/LAPACK接口标准实现，同是有两种可能是避免报错？目前只了解without部分。

2.1、matmul()

这个函数是写的一个整合的矩阵相乘

/* multiply matrix (wrapper of blas dgemm) -------------------------------------
* multiply matrix by matrix (C=alpha*A*B+beta*C)
* args   : char   *tr       I  transpose flags ("N":normal,"T":transpose)
*          int    n,k,m     I  size of (transposed) matrix A,B
*          double alpha     I  alpha
*          double *A,*B     I  (transposed) matrix A (n x m), B (m x k)
*          double beta      I  beta
*          double *C        IO matrix C (n x k)
* return : none
*-----------------------------------------------------------------------------*/
extern void matmul(const char *tr, int n, int k, int m, double alpha,const double *A, const double *B, double beta, double *C)
{int lda=tr[0]=='T'?m:n,ldb=tr[1]=='T'?k:m;dgemm_((char *)tr,(char *)tr+1,&n,&k,&m,&alpha,(double *)A,&lda,(double *)B,&ldb,&beta,C,&n);
}
/* 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);//首先分类，通过传入的第一个参数，判定两个字符是否为N，即是否为正常矩阵，然后传数字给下面进行计算for (i=0;i<n;i++) for (j=0;j<k;j++) {//n和k是用来判断循环次数，因此n、k分别为第一个矩阵的行和第二个矩阵的列，// 而第一个矩阵的列于第二个矩阵的行公用，所以只需要三个参数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.2、ludcmp()

LU分解，即把矩阵A分解LU乘积的形式，U为单位上三角矩阵和L单位为下三角矩阵两部分

/* 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;
}

2.3、lubksb()

LU回代,即把单位上三角矩阵U和单位下三角矩阵L矩阵回代为一个整体矩阵

/* 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];}
}

2.4、matinv()

求矩阵的逆

/* inverse of matrix -----------------------------------------------------------
* inverse of matrix (A=A^-1)
* args   : double *A        IO  matrix (n x n)
*          int    n         I   size of matrix A
* return : status (0:ok,0>:error)
*-----------------------------------------------------------------------------*/
extern int matinv(double *A, int n)
{double *work;int info,lwork=n*16,*ipiv=imat(n,1);work=mat(lwork,1);dgetrf_(&n,&n,A,&n,ipiv,&info);if (!info) dgetri_(&n,A,&n,ipiv,work,&lwork,&info);free(ipiv); free(work);return info;
}
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;}//如果B不能LU分解，则报错？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);//还未了解LU分解及回代}free(indx); free(B);return 0;
}

2.5、 solve()

求方程线性解

/* solve linear equation -------------------------------------------------------
* solve linear equation (X=A\Y or X=A'\Y)
* args   : char   *tr       I   transpose flag ("N":normal,"T":transpose)
*          double *A        I   input matrix A (n x n)
*          double *Y        I   input matrix Y (n x m)
*          int    n,m       I   size of matrix A,Y
*          double *X        O   X=A\Y or X=A'\Y (n x m)
* return : status (0:ok,0>:error)
* notes  : matirix stored by column-major order (fortran convention)
*          X can be same as Y
*-----------------------------------------------------------------------------*/
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,*ipiv=imat(n,1);matcpy(B,A,n,n);matcpy(X,Y,n,m);dgetrf_(&n,&n,B,&n,ipiv,&info);if (!info) dgetrs_((char *)tr,&n,&m,B,&n,ipiv,X,&n,&info);free(ipiv); free(B); return info;
}
/* 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);//复制A矩阵给Bif (!(info=matinv(B,n))) matmul(tr[0]=='N'?"NN":"TN",n,m,n,1.0,B,Y,0.0,X);//首先判断B是否可逆，然后判断B是正常还是转置矩阵，传给matmul计算free(B);return info;
}