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

// d2c.cpp// 完全自包含的 MATLAB d2c (ZOH) 实现// 编译: g -stdc17 -O2 d2c.cpp -o d2c#includeiostream#includeiomanip#includecomplex#includevector#includecmath#includestdexcept#includestringusingcxstd::complexdouble;// // 复数矩阵类行优先扁平化存储// structCMat{introws,cols;std::vectorcxa;CMat():rows(0),cols(0){}CMat(intr,intc):rows(r),cols(c),a(static_castsize_t(r)*c,cx(0.0,0.0)){}cxoperator()(inti,intj){returna[static_castsize_t(i)*colsj];}cxoperator()(inti,intj)const{returna[static_castsize_t(i)*colsj];}staticCMateye(intn){CMatI(n,n);for(inti0;in;i)I(i,i)cx(1.0,0.0);returnI;}};// ----- 基本算术 -----CMatoperator(constCMatA,constCMatB){CMatC(A.rows,A.cols);for(size_t i0;iA.a.size();i)C.a[i]A.a[i]B.a[i];returnC;}CMatoperator-(constCMatA,constCMatB){CMatC(A.rows,A.cols);for(size_t i0;iA.a.size();i)C.a[i]A.a[i]-B.a[i];returnC;}CMatoperator*(constCMatA,constCMatB){if(A.cols!B.rows)throwstd::runtime_error(matmul: dim mismatch);CMatC(A.rows,B.cols);for(inti0;iA.rows;i)for(intk0;kA.cols;k){cx aikA(i,k);if(std::abs(aik)1e-300)continue;for(intj0;jB.cols;j)C(i,j)aik*B(k,j);}returnC;}CMatoperator*(cx s,constCMatA){CMatC(A.rows,A.cols);for(size_t i0;iA.a.size();i)C.a[i]s*A.a[i];returnC;}CMatoperator*(constCMatA,cx s){CMatC(A.rows,A.cols);for(size_t i0;iA.a.size();i)C.a[i]A.a[i]*s;returnC;}// Frobenius 范数doublefnorm(constCMatA){doubles0.0;for(constautoz:A.a)sstd::norm(z);returnstd::sqrt(s);}// // 高斯-约当求逆带部分选主元// CMatinverse(constCMatA){intnA.rows;if(n!A.cols)throwstd::runtime_error(inverse: not square);CMat MA;CMat ICMat::eye(n);for(intcol0;coln;col){// 选主元intpivcol;doublemxstd::abs(M(col,col));for(intrcol1;rn;r){doublevstd::abs(M(r,col));if(vmx){mxv;pivr;}}if(mx1e-14)throwstd::runtime_error(inverse: singular matrix);if(piv!col){for(intj0;jn;j){std::swap(M(col,j),M(piv,j));std::swap(I(col,j),I(piv,j));}}cx pM(col,col);for(intj0;jn;j){M(col,j)/p;I(col,j)/p;}for(intr0;rn;r){if(rcol)continue;cx fM(r,col);if(std::abs(f)1e-300)continue;for(intj0;jn;j){M(r,j)-f*M(col,j);I(r,j)-f*I(col,j);}}}returnI;}// // Denman-Beavers 迭代求主平方根// Y_{k1} 0.5 (Y_k Z_k^{-1})// Z_{k1} 0.5 (Z_k Y_k^{-1})// CMatmat_sqrt(constCMatA,intmax_it80,doubletol1e-15){intnA.rows;CMat YA;CMat ZCMat::eye(n);for(intk0;kmax_it;k){CMat Yiinverse(Y);CMat Ziinverse(Z);CMat Yn0.5*(YZi);CMat Zn0.5*(ZYi);doubleerrfnorm(Yn-Y);YYn;ZZn;if(errtol)break;}returnY;}// // 矩阵对数 log(A)A 的特征值不位于负实轴上// 方法: scaling-and-squaring Taylor 级数// log(A) 2^s * log(A^{1/2^s})// log(I N) N - N^2/2 N^3/3 - ...// CMatmat_log(constCMatA){intnA.rows;CMat ICMat::eye(n);CMat XA;ints0;// ---- 缩放: 反复开方直到接近单位阵 ----while(fnorm(X-I)0.5s30){Xmat_sqrt(X);s;}// ---- Taylor 级数 ----CMat NX-I;CMatR(n,n);CMat NkI;// N^0for(intk1;k100;k){NkNk*N;// N^kdoublecoef((k%2)1)?(1.0/k):(-1.0/k);RRcx(coef,0.0)*Nk;if(fnorm(Nk)/k1e-20)break;}// ---- 缩放回原尺度 ----Rstd::pow(2.0,s)*R;returnR;}// // 实矩阵接口// usingRMatstd::vectorstd::vectordouble;// d2c (ZOH 方法)// 输入: Phi (n×n), Gamma (n×m), Ts// 输出: Ac (n×n), Bc (n×m)voidd2c_zoh(constRMatPhi,constRMatGamma,doubleTs,RMatAc,RMatBc){intnstatic_castint(Phi.size());intmstatic_castint(Gamma[0].size());// ---- 构造增广矩阵 M [ Phi Gamma ; 0 I ] ----CMatM(nm,nm);for(inti0;in;i){for(intj0;jn;j)M(i,j)Phi[i][j];for(intj0;jm;j)M(i,nj)Gamma[i][j];}for(inti0;im;i)M(ni,ni)1.0;// ---- L log(M) / Ts ----CMat Lmat_log(M);L(1.0/Ts)*L;// ---- 提取 A、B取实部 ----Ac.assign(n,std::vectordouble(n));Bc.assign(n,std::vectordouble(m));for(inti0;in;i){for(intj0;jn;j)Ac[i][j]L(i,j).real();for(intj0;jm;j)Bc[i][j]L(i,nj).real();}}// ---- 打印辅助 ----voidprint_mat(conststd::stringname,constRMatA){std::coutname:\n;for(constautorow:A){for(doublev:row)std::coutstd::setw(12)v ;std::cout\n;}}// // 主程序: 演示// intmain(){std::coutstd::fixedstd::setprecision(6);// ---------- 测试 1: 二阶系统 ----------RMat Phi1{{0.9,0.1},{0.0,0.8}};RMat Gam1{{1.0},{0.5}};doubleTs0.1;std::cout 测试 1: 二阶系统 d2c (ZOH) \n;print_mat(离散 Phi,Phi1);print_mat(离散 Gamma,Gam1);std::coutTs Ts\n\n;RMat Ac1,Bc1;d2c_zoh(Phi1,Gam1,Ts,Ac1,Bc1);print_mat(连续 A,Ac1);print_mat(连续 B,Bc1);// ---------- 测试 2: Phi I 特殊情况 ----------RMat Phi2{{1.0}};RMat Gam2{{2.0}};std::cout\n 测试 2: Phi I 特殊情况 \n;RMat Ac2,Bc2;d2c_zoh(Phi2,Gam2,Ts,Ac2,Bc2);print_mat(连续 A (理论 0),Ac2);print_mat(连续 B (理论 Gamma/Ts 20),Bc2);// ---------- 测试 3: 标量系统 ----------RMat Phi3{{0.5}};RMat Gam3{{1.0}};std::cout\n 测试 3: 标量系统 \n;RMat Ac3,Bc3;d2c_zoh(Phi3,Gam3,Ts,Ac3,Bc3);print_mat(连续 A,Ac3);print_mat(连续 B,Bc3);std::cout理论 A log(0.5)/Ts std::log(0.5)/Ts\n;std::cout理论 B Gamma*A/(Phi-1) 1.0*(std::log(0.5)/Ts)/(0.5-1.0)\n;return0;}