#includeiostream#includevector#includestring#includesstream#includeiomanip#includecmath#includealgorithm#includestdexcept// // 自实现的轻量矩阵类替代 Eigen// classMatrix{public:introws,cols;std::vectorstd::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc,doubleval0.0):rows(r),cols(c),data(r,std::vectordouble(c,val)){}// 支持 Matrix A {{1,2},{3,4}}; 形式构造Matrix(std::initializer_liststd::initializer_listdoubleinit){rowsstatic_castint(init.size());colsrows?static_castint(init.begin()-size()):0;data.resize(rows);inti0;for(constautorow:init){data[i].assign(row.begin(),row.end());if(static_castint(data[i].size())!cols)throwstd::invalid_argument(Ragged matrix initializer.);i;}}doubleoperator()(inti,intj){returndata[i][j];}constdoubleoperator()(inti,intj)const{returndata[i][j];}// 矩阵乘法Matrixoperator*(constMatrixB)const{if(cols!B.rows)throwstd::invalid_argument(Matrix multiply: dimension mismatch.);MatrixR(rows,B.cols,0.0);for(inti0;irows;i){for(intk0;kcols;k){doubleaikdata[i][k];if(aik0.0)continue;for(intj0;jB.cols;j)R.data[i][j]aik*B.data[k][j];}}returnR;}// 把 B 写入当前矩阵从 (r0, c0) 开始的子块voidsetBlock(intr0,intc0,constMatrixB){for(inti0;iB.rows;i)for(intj0;jB.cols;j)data[r0i][c0j]B.data[i][j];}};// // 矩阵打印每列按该列最大宽度右对齐列间以单空格分隔// staticstd::stringnumToStr(doublev){if(std::fabs(v-std::round(v))1e-9std::fabs(v)1e12){std::ostringstream oss;ossstatic_castlonglong(std::llround(v));returnoss.str();}std::ostringstream oss;ossv;returnoss.str();}std::ostreamoperator(std::ostreamos,constMatrixM){std::vectorintwidth(M.cols,0);std::vectorstd::vectorstd::stringstrs(M.rows,std::vectorstd::string(M.cols));for(inti0;iM.rows;i)for(intj0;jM.cols;j){strs[i][j]numToStr(M.data[i][j]);width[j]std::max(width[j],static_castint(strs[i][j].size()));}for(inti0;iM.rows;i){for(intj0;jM.cols;j){if(j)os ;osstd::setw(width[j])strs[i][j];}os\n;}returnos;}// // obsv计算能观性矩阵 O [C; CA; CA^2; \...; CA^(n-1)]// Matrixobsv(constMatrixA,constMatrixC){if(A.rows!A.cols)throwstd::invalid_argument(A must be square.);if(C.cols!A.rows)throwstd::invalid_argument(C columns must equal A rows.);intnA.rows;// 状态维数intpC.rows;// 输出维数MatrixO(p*n,n,0.0);Matrix CAC;for(inti0;in;i){O.setBlock(i*p,0,CA);if(in-1)CACA*A;// 递推CA - C * A^(i1)}returnO;}// // 高斯消元求矩阵秩用于判断能观性// intmatrixRank(Matrix M){intmM.rows,nM.cols;intrank0;constdoubleeps1e-9;for(intc0;cnrankm;c){// 选列主元intpivot-1;doublebesteps;for(intrrank;rm;r){doublevstd::fabs(M(r,c));if(vbest){bestv;pivotr;}}if(pivot-1)continue;std::swap(M.data[pivot],M.data[rank]);for(intrrank1;rm;r){doublefactorM(r,c)/M(rank,c);if(factor0.0)continue;for(intkc;kn;k)M(r,k)-factor*M(rank,k);}rank;}returnrank;}// // 测试主程序// intmain(){Matrix A{{1,2},{3,4}};Matrix C{{5,6}};Matrix Oobsv(A,C);std::coutObservability Matrix:\nO;intrankmatrixRank(O);std::coutRank: rank (State dimension: A.rows)\n;if(rankA.rows)std::coutSystem is observable.\n;elsestd::coutSystem is NOT observable.\n;return0;}