最近要做一個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;