最近要做一个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上一篇博客： 改了一下
/********参数表*******
* @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;
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
