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

#includeiostream#includevector#includerandom#includecomplex#includecmath#includestdexcept// 使用行主序存储矩阵matrix[i][j]usingMatrixstd::vectorstd::vectordouble;structStateSpace{Matrix A,B,C,D;intn,m,p;// n-状态, m-输入, p-输出};// 矩阵乘法: C A * BMatrixmatmul(constMatrixA,constMatrixB){intnA.size(),mA[0].size(),pB[0].size();MatrixC(n,std::vectordouble(p,0.0));for(inti0;in;i)for(intk0;km;k)for(intj0;jp;j)C[i][j]A[i][k]*B[k][j];returnC;}// 矩阵求逆 (高斯-约旦消元)Matrixmatinv(constMatrixA){intnA.size();Matrixaug(n,std::vectordouble(2*n,0.0));for(inti0;in;i){for(intj0;jn;j)aug[i][j]A[i][j];aug[i][ni]1.0;// 右半区构造单位阵}for(inti0;in;i){intpivoti;for(intji1;jn;j)if(std::abs(aug[j][i])std::abs(aug[pivot][i]))pivotj;if(std::abs(aug[pivot][i])1e-12)throwstd::runtime_error(Matrix is singular);std::swap(aug[i],aug[pivot]);doubledivaug[i][i];for(intj0;j2*n;j)aug[i][j]/div;for(intj0;jn;j){if(ji)continue;doublefactoraug[j][i];for(intk0;k2*n;k)aug[j][k]-factor*aug[i][k];}}Matrixinv(n,std::vectordouble(n));for(inti0;in;i)for(intj0;jn;j)inv[i][j]aug[i][jn];returninv;}// 生成 drss 随机离散状态空间模型StateSpacedrss(intn,intp,intm,unsignedseed0){StateSpace sys;sys.nn;sys.pp;sys.mm;std::mt19937rng(seed?seed:std::random_device{}());std::uniform_real_distributiondoubleuniform(0.0,1.0);// 1. 生成稳定极点std::vectorstd::complexdoublepoles;doublemagLow0.5,magHigh0.97;doublepReal0.6,pRepeat0.05;inti0;while(in){if(i0in-1uniform(rng)pRepeat){if(std::abs(poles.back().imag())1e-12){poles.push_back(poles.back());i;}else{poles.push_back(poles[poles.size()-2]);poles.push_back(poles.back());i2;}}elseif(uniform(rng)pReal||in-1){doublemagmagLow(magHigh-magLow)*uniform(rng);poles.emplace_back(mag,0.0);i;}else{doublemagmagLow(magHigh-magLow)*uniform(rng);doublephase(std::acos(-1.0)/2)*uniform(rng);doubleremag*std::cos(phase);doubleimmag*std::sin(phase);poles.emplace_back(re,im);poles.emplace_back(re,-im);i2;}}// 2. 构造块对角矩阵 A_diagMatrixAd(n,std::vectordouble(n,0.0));i0;while(in){if(std::abs(poles[i].imag())1e-12){Ad[i][i]poles[i].real();i;}else{Ad[i][i]poles[i].real();Ad[i1][i1]poles[i].real();Ad[i][i1]poles[i].imag();Ad[i1][i]-poles[i].imag();i2;}}// 3. 随机相似变换: A T * Ad * inv(T)std::normal_distributiondoublenormal(0.0,1.0);MatrixT(n,std::vectordouble(n));for(intr0;rn;r)for(intc0;cn;c)T[r][c]normal(rng);Matrix Tinv;try{Tinvmatinv(T);}catch(...){TinvMatrix(n,std::vectordouble(n,0.0));for(intr0;rn;r)Tinv[r][r]1.0;TTinv;}sys.Amatmul(matmul(T,Ad),Tinv);// 4. 随机生成 B (n x m)doublepBCmask0.8;sys.B.assign(n,std::vectordouble(m,0.0));for(intr0;rn;r)for(intc0;cm;c)if(uniform(rng)pBCmask)sys.B[r][c]normal(rng);// 5. 随机生成 C (p x n)sys.C.assign(p,std::vectordouble(n,0.0));for(intr0;rp;r)for(intc0;cn;c)if(uniform(rng)pBCmask)sys.C[r][c]normal(rng);// 6. 随机生成 D (p x m)doublepDmask0.3,pDzero0.5;sys.D.assign(p,std::vectordouble(m,0.0));for(intr0;rp;r)for(intc0;cm;c){if(uniform(rng)pDzero)sys.D[r][c]0.0;elseif(uniform(rng)pDmask)sys.D[r][c]normal(rng);}returnsys;}// 打印矩阵voidprintMatrix(constMatrixM,conststd::stringname){std::coutname \n;for(autorow:M){for(doublev:row)std::coutv ;std::cout\n;}std::cout\n;}intmain(){// 示例3 状态, 4 输出, 2 输入autosysdrss(3,4,2,42);std::cout离散随机状态空间模型 (n3, p4, m2)\n;std::cout状态数: sys.n, 输出数: sys.p, 输入数: sys.m\n\n;printMatrix(sys.A,A);printMatrix(sys.B,B);printMatrix(sys.C,C);printMatrix(sys.D,D);return0;}