最近要做一个MFC的上位机，用到CSP滤波算法，这玩意儿在MATLAB 里相当简单就能实现但C里面实现起来太蛋疼，写了一个晚上才把这个算法用到的矩阵运算部分的函数写的差不多，为了避免以后再重复造轮子，现在这里写一下备份一下吧。。
1.矩阵乘法
//矩阵乘法 /********参数表******* @Parameter x: m行k列矩阵（用一维数组表示） @Parameter y: k行n列矩阵（用一维数组表示） @Parameter m,k,n: 矩阵行列参数 @Parameter z: m行n列输出矩阵（用一维数组表示） ***********************/ void MulMatrixDD(double *x,double *y, int m,int k,int n, double *z) { for(int nm=0; nm<m; nm++) for(int nn=0; nn<n; nn++) for(int nk=0; nk<k; nk++) z[nm*n+nn] += x[nm*k+nk]*y[nk*n+nn]; }
因为输入的x,y可能是不同的类型（short, double）所以我只简单的复制了几个函数来表示，比如输入如果是x如果是short型，y是double型这个函数就不好用了。如果有什么更好的通用方法可以跟我交流下
2.方阵转置
//方阵转置 /********参数表******* @Parameter x: m行m列矩阵（用一维数组表示） @Parameter m: 矩阵行列数 ***********************/ void TransSquareD(double *x, int m) { double temp; for(int nm=0; nm<m; nm++){ //对原矩阵第nm行 for(int nn=0; nn<nm; nn++){ //对原矩阵第nn列 temp = x[nm*m+nn]; //z矩阵第nn行第nm列 x[nm*m+nn] = x[nn*m+nm]; x[nn*m+nm] = temp;}} }
3.非方阵转置
//非方阵转置 /********参数表******* @Parameter x: m行n列矩阵（用一维数组表示） @Parameter m,n: 矩阵行列数 @Parameter z: n行m列矩阵（用一维数组表示） ***********************/ void TransMatrixD(double *x, int m, int n, double *z) { for(int nm=0; nm<m; nm++) //对原矩阵第nm行 for(int nn=0; nn<n; nn++) //对原矩阵第nn列 z[nn*m+nm] = x[nm*n+nn]; //z矩阵第nn行第nm列 } void TransMatrixS(short *x, int m, int n, double *z) { for(int nm=0; nm<m; nm++) //对原矩阵第nm行 for(int nn=0; nn<n; nn++) //对原矩阵第nn列 z[nn*m+nm] = (double)x[nm*n+nn]; //z矩阵第nn行第nm列 }
4.协方差矩阵
//协方差矩阵函数 /********参数表******* @Parameter X[m_cov][n_cov]: m_cov行n_cov列矩阵（用二维数组表示） ***********************/ void CovMat(short X[m_cov][n_cov]) { for(int i=0; i<m_cov; i++) for(int j=0; j<m_cov; j++) CovMatrix[i][j] = cov(*(X+i),*(X+j)); } //协方差函数 double cov(short *x, short *y) { double sumx = 0; double sumy = 0; double sum = 0; int kk = 0; for(kk = 0; kk<n_cov; kk++) { sumx += x[kk]; sumy += y[kk]; } sumx /= n_cov; sumy /= n_cov; for(kk = 0; kk<n_cov; kk++) { sum += (x[kk]-sumx)*(y[kk]-sumy); } sum /= (n_cov-1); return sum; }
5.求矩阵特征值和特征向量
这个短时间自己造轮子太麻烦了，，参考csdn上一篇博客：https://blog.csdn.net/zhouxuguang236/article/details/40212143 改了一下
/********参数表******* * @brief 求实对称矩阵的特征值及特征向量的雅克比法 * 利用雅格比(Jacobi)方法求实对称矩阵的全部特征值及特征向量 @Parameter pMatrix 长度为n*n的数组，存放实对称矩阵 @Parameter nDim 矩阵的阶数 @Parameter pdblVects 长度为n*n的数组，返回特征向量(按列存储) @Parameter dbEps 精度要求 @Parameter nJt 整型变量，控制最大迭代次数 @Parameter pdbEigenValues 特征值数组 ***********************/ void JacbiCor(double * pMatrix,int nDim, double *pdblVects, double *pdbEigenValues, double dbEps,int nJt) { int i,j; for(i = 0; i < nDim; i ++) { pdblVects[i*nDim+i] = 1.0f; for(int j = 0; j < nDim; j ++) { if(i != j) pdblVects[i*nDim+j]=0.0f; } } int nCount = 0; //迭代次数 while(1) { //在pMatrix的非对角线上找到最大元素 double dbMax = pMatrix[1]; int nRow = 0; int nCol = 1; for (i = 0; i < nDim; i ++) //行 { for (j = 0; j < nDim; j ++) //列 { double d = fabs(pMatrix[i*nDim+j]); if((i!=j) && (d> dbMax)) { dbMax = d; nRow = i; nCol = j; } } } if(dbMax < dbEps) //精度符合要求 break; if(nCount > nJt) //迭代次数超过限制 break; nCount++; double dbApp = pMatrix[nRow*nDim+nRow]; double dbApq = pMatrix[nRow*nDim+nCol]; double dbAqq = pMatrix[nCol*nDim+nCol]; //计算旋转角度 double dbAngle = 0.5*atan2(-2*dbApq,dbAqq-dbApp); double dbSinTheta = sin(dbAngle); double dbCosTheta = cos(dbAngle); double dbSin2Theta = sin(2*dbAngle); double dbCos2Theta = cos(2*dbAngle); pMatrix[nRow*nDim+nRow] = dbApp*dbCosTheta*dbCosTheta + dbAqq*dbSinTheta*dbSinTheta + 2*dbApq*dbCosTheta*dbSinTheta; pMatrix[nCol*nDim+nCol] = dbApp*dbSinTheta*dbSinTheta + dbAqq*dbCosTheta*dbCosTheta - 2*dbApq*dbCosTheta*dbSinTheta; pMatrix[nRow*nDim+nCol] = 0.5*(dbAqq-dbApp)*dbSin2Theta + dbApq*dbCos2Theta; pMatrix[nCol*nDim+nRow] = pMatrix[nRow*nDim+nCol]; for(i = 0; i < nDim; i ++) { if((i!=nCol) && (i!=nRow)) { int u = i*nDim + nRow; //p int w = i*nDim + nCol; //q dbMax = pMatrix[u]; pMatrix[u]= pMatrix[w]*dbSinTheta + dbMax*dbCosTheta; pMatrix[w]= pMatrix[w]*dbCosTheta - dbMax*dbSinTheta; } } for (j = 0; j < nDim; j ++) { if((j!=nCol) && (j!=nRow)) { int u = nRow*nDim + j; //p int w = nCol*nDim + j; //q dbMax = pMatrix[u]; pMatrix[u]= pMatrix[w]*dbSinTheta + dbMax*dbCosTheta; pMatrix[w]= pMatrix[w]*dbCosTheta - dbMax*dbSinTheta; } } //计算特征向量 for(i = 0; i < nDim; i ++) { int u = i*nDim + nRow; //p int w = i*nDim + nCol; //q dbMax = pdblVects[u]; pdblVects[u] = pdblVects[w]*dbSinTheta + dbMax*dbCosTheta; pdblVects[w] = pdblVects[w]*dbCosTheta - dbMax*dbSinTheta; } } for(i = 0; i < nDim; i ++) { pdbEigenValues[i] = pMatrix[i*nDim+i]; } //设定正负号 for(i = 0; i < nDim; i ++) { double dSumVec = 0; for(j = 0; j < nDim; j ++) dSumVec += pdblVects[j * nDim + i]; if(dSumVec<0) { for(j = 0;j < nDim; j ++) pdblVects[j * nDim + i] *= -1; } } }
二维数组转一维数组：
const short m_cov = 3; const short n_cov = 3; short X[m_cov][n_cov] = {0}; short *XFlat; /************X测试************/ X[0][0] = 7;X[0][1] = 1;X[0][2] = 8; X[1][0] = 4;X[1][1] = 5;X[1][2] = 8; X[2][0] = 10;X[2][1] = 4;X[2][2] = 2; /*****************************/ XFlat = (short *)X;