#includeiostream#includevector#includecmath#includelimits#includestdexcept#includealgorithm#includecstddef// 简易矩阵类 classMatrix{public:Matrix()default;Matrix(intr,intc):rows_(r),cols_(c),data_(static_caststd::size_t(r)*c,0.0){}introws()const{returnrows_;}intcols()const{returncols_;}doubleoperator()(inti,intj){returndata_[static_caststd::size_t(i)*cols_j];}doubleoperator()(inti,intj)const{returndata_[static_caststd::size_t(i)*cols_j];}private:introws_0;intcols_0;std::vectordoubledata_;};// 矩阵乘法 Matrixoperator*(constMatrixA,constMatrixB){if(A.cols()!B.rows())throwstd::invalid_argument(matrix multiply: dimension mismatch);MatrixC(A.rows(),B.cols());for(inti0;iA.rows();i)for(intk0;kA.cols();k){doubleaA(i,k);if(a0.0)continue;for(intj0;jB.cols();j)C(i,j)a*B(k,j);}returnC;}// 可控性矩阵 C [B, AB, ..., A^(n-1)B] Matrixctrb(constMatrixA,constMatrixB){if(A.rows()!A.cols())throwstd::invalid_argument(ctrb: A must be square);if(B.rows()!A.rows())throwstd::invalid_argument(ctrb: B must have same row count as A);constintnA.rows();constintmB.cols();MatrixCo(n,n*m);// 第 0 块 Bfor(inti0;in;i)for(intj0;jm;j)Co(i,j)B(i,j);Matrix PB;for(intk1;kn;k){PA*P;// P A^k * Bfor(inti0;in;i)for(intj0;jm;j)Co(i,k*mj)P(i,j);}returnCo;}// 数值秩全主元高斯消元 intmatrixRank(constMatrixM){constintmM.rows();constintnM.cols();if(m0||n0)return0;Matrix aM;// 最大绝对值用于设定相对阈值doublemaxAbs0.0;for(inti0;im;i)for(intj0;jn;j)maxAbsstd::max(maxAbs,std::fabs(a(i,j)));if(maxAbs0.0)return0;constdoubletolstd::max(m,n)*std::numeric_limitsdouble::epsilon()*maxAbs;intr0;constintkMaxstd::min(m,n);for(intk0;kkMax;k){// 在当前子块中找绝对值最大的元素作主元intpik,pjk;doublebest0.0;for(intik;im;i)for(intjk;jn;j){doublevstd::fabs(a(i,j));if(vbest){bestv;pii;pjj;}}if(besttol)break;// 剩余子块可视为零// 交换行if(pi!k)for(intj0;jn;j)std::swap(a(k,j),a(pi,j));// 交换列if(pj!k)for(inti0;im;i)std::swap(a(i,k),a(i,pj));r;constdoublepiva(k,k);for(intik1;im;i){constdoublefa(i,k)/piv;if(f0.0)continue;for(intjk;jn;j)a(i,j)-f*a(k,j);}}returnr;}// 可控性判断 structControllabilityResult{Matrix Co;intrank;boolisControllable;};ControllabilityResultcheckControllability(constMatrixA,constMatrixB){Matrix Coctrb(A,B);constintrmatrixRank(Co);return{Co,r,rA.rows()};}// 矩阵打印 voidprintMatrix(constMatrixM){for(inti0;iM.rows();i){for(intj0;jM.cols();j){if(j)std::cout ;doublevM(i,j);doublerstd::round(v);// 接近整数时按整数打印便于与 MATLAB 输出对照if(std::fabs(v-r)1e-9*std::max(1.0,std::fabs(v))std::fabs(r)1e15)std::coutstatic_castlonglong(r);elsestd::coutv;}std::cout\n;}}// 示例 intmain(){MatrixA(2,2);A(0,0)1;A(0,1)1;A(1,0)4;A(1,1)-2;MatrixB(2,2);B(0,0)1;B(0,1)-1;B(1,0)1;B(1,1)-1;autoresultcheckControllability(A,B);std::coutControllability matrix:\n;printMatrix(result.Co);std::cout\nrank result.rank\n;std::coutcontrollable std::boolalpharesult.isControllable\n;return0;}