C++代码实现MATLAB中的ctrb函数功能

#include<iostream>#include<vector>#include<cmath>#include<limits>#include<stdexcept>#include<algorithm>#include<cstddef>// ===================== 简易矩阵类 =====================classMatrix{public:Matrix()=default;Matrix(intr,intc):rows_(r),cols_(c),data_(static_cast<std::size_t>(r)*c,0.0){}introws()const{returnrows_;}intcols()const{returncols_;}double&operator()(inti,intj){returndata_[static_cast<std::size_t>(i)*cols_+j];}doubleoperator()(inti,intj)const{returndata_[static_cast<std::size_t>(i)*cols_+j];}private:introws_=0;intcols_=0;std::vector<double>data_;};// ===================== 矩阵乘法 =====================Matrixoperator*(constMatrix&A,constMatrix&B){if(A.cols()!=B.rows())throwstd::invalid_argument("matrix multiply: dimension mismatch");MatrixC(A.rows(),B.cols());for(inti=0;i<A.rows();++i)for(intk=0;k<A.cols();++k){doublea=A(i,k);if(a==0.0)continue;for(intj=0;j<B.cols();++j)C(i,j)+=a*B(k,j);}returnC;}// ===================== 可控性矩阵 C = [B, AB, ..., A^(n-1)B] =====================Matrixctrb(constMatrix&A,constMatrix&B){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");constintn=A.rows();constintm=B.cols();MatrixCo(n,n*m);// 第 0 块 = Bfor(inti=0;i<n;++i)for(intj=0;j<m;++j)Co(i,j)=B(i,j);Matrix P=B;for(intk=1;k<n;++k){P=A*P;// P = A^k * Bfor(inti=0;i<n;++i)for(intj=0;j<m;++j)Co(i,k*m+j)=P(i,j);}returnCo;}// ===================== 数值秩（全主元高斯消元） =====================intmatrixRank(constMatrix&M){constintm=M.rows();constintn=M.cols();if(m==0||n==0)return0;Matrix a=M;// 最大绝对值，用于设定相对阈值doublemaxAbs=0.0;for(inti=0;i<m;++i)for(intj=0;j<n;++j)maxAbs=std::max(maxAbs,std::fabs(a(i,j)));if(maxAbs==0.0)return0;constdoubletol=std::max(m,n)*std::numeric_limits<double>::epsilon()*maxAbs;intr=0;constintkMax=std::min(m,n);for(intk=0;k<kMax;++k){// 在当前子块中找绝对值最大的元素作主元intpi=k,pj=k;doublebest=0.0;for(inti=k;i<m;++i)for(intj=k;j<n;++j){doublev=std::fabs(a(i,j));if(v>best){best=v;pi=i;pj=j;}}if(best<=tol)break;// 剩余子块可视为零// 交换行if(pi!=k)for(intj=0;j<n;++j)std::swap(a(k,j),a(pi,j));// 交换列if(pj!=k)for(inti=0;i<m;++i)std::swap(a(i,k),a(i,pj));++r;constdoublepiv=a(k,k);for(inti=k+1;i<m;++i){constdoublef=a(i,k)/piv;if(f==0.0)continue;for(intj=k;j<n;++j)a(i,j)-=f*a(k,j);}}returnr;}// ===================== 可控性判断 =====================structControllabilityResult{Matrix Co;intrank;boolisControllable;};ControllabilityResultcheckControllability(constMatrix&A,constMatrix&B){Matrix Co=ctrb(A,B);constintr=matrixRank(Co);return{Co,r,r==A.rows()};}// ===================== 矩阵打印 =====================voidprintMatrix(constMatrix&M){for(inti=0;i<M.rows();++i){for(intj=0;j<M.cols();++j){if(j)std::cout<<' ';doublev=M(i,j);doubler=std::round(v);// 接近整数时按整数打印，便于与 MATLAB 输出对照if(std::fabs(v-r)<1e-9*std::max(1.0,std::fabs(v))&&std::fabs(r)<1e15)std::cout<<static_cast<longlong>(r);elsestd::cout<<v;}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;autoresult=checkControllability(A,B);std::cout<<"Controllability matrix:\n";printMatrix(result.Co);std::cout<<"\nrank = "<<result.rank<<"\n";std::cout<<"controllable = "<<std::boolalpha<<result.isControllable<<"\n";return0;}