// dlyap.cpp// 求解离散 Lyapunov 方程: A * X * A^T - X Q 0// 仅使用 C 标准库无第三方依赖#includeiostream#includevector#includecmath#includestdexcept#includeiomanip#includealgorithmusingMatrixstd::vectorstd::vectordouble;usingVectorstd::vectordouble;// ---------------- 基础矩阵运算 ----------------staticMatrixmatmul(constMatrixA,constMatrixB){intn(int)A.size(),k(int)B.size(),m(int)B[0].size();MatrixC(n,Vector(m,0.0));for(inti0;in;i)for(intl0;lk;l){doubleaA[i][l];if(a0.0)continue;for(intj0;jm;j)C[i][j]a*B[l][j];}returnC;}staticMatrixtranspose(constMatrixA){intn(int)A.size(),m(int)A[0].size();MatrixT(m,Vector(n));for(inti0;in;i)for(intj0;jm;j)T[j][i]A[i][j];returnT;}// ---------------- 高斯消元部分主元 ----------------staticVectorsolveLinear(Matrix M,Vector b){intn(int)M.size();for(inti0;in;i){intpivi;for(intki1;kn;k)if(std::fabs(M[k][i])std::fabs(M[piv][i]))pivk;if(std::fabs(M[piv][i])1e-14)throwstd::runtime_error(dlyap(): singular system (solution not unique));std::swap(M[i],M[piv]);std::swap(b[i],b[piv]);doublediagM[i][i];for(intki1;kn;k){doublefM[k][i]/diag;if(f0.0)continue;for(intji;jn;j)M[k][j]-f*M[i][j];b[k]-f*b[i];}}Vectorx(n);for(intin-1;i0;--i){doublesb[i];for(intji1;jn;j)s-M[i][j]*x[j];x[i]s/M[i][i];}returnx;}// ---------------- Jacobi 特征值仅用于对对称矩阵求特征值 ----------------staticVectorjacobiEigenvalues(Matrix A){intn(int)A.size();for(intsweep0;sweep100;sweep){doubleoff0.0;for(inti0;in;i)for(intji1;jn;j)offA[i][j]*A[i][j];if(off1e-24)break;for(intp0;pn;p){for(intqp1;qn;q){if(std::fabs(A[p][q])1e-18)continue;doubletheta0.5*(A[q][q]-A[p][p])/A[p][q];doublet(theta0?1.0:-1.0)/(std::fabs(theta)std::sqrt(theta*theta1.0));doublec1.0/std::sqrt(t*t1.0);doublest*c;doubleappA[p][p],aqqA[q][q],apqA[p][q];A[p][p]app-t*apq;A[q][q]aqqt*apq;A[p][q]A[q][p]0.0;for(intk0;kn;k){if(kp||kq)continue;doubleakpA[k][p],akqA[k][q];A[k][p]c*akp-s*akq;A[k][q]s*akpc*akq;A[p][k]A[k][p];A[q][k]A[k][q];}}}}Vectoreigs(n);for(inti0;in;i)eigs[i]A[i][i];std::sort(eigs.begin(),eigs.end());returneigs;}// ---------------- dlyap 主函数 ----------------staticMatrixdlyap(constMatrixA,constMatrixQ){constintn(int)A.size();if((int)A[0].size()!n||(int)Q.size()!n||(int)Q[0].size()!n)throwstd::runtime_error(dlyap(): A and Q must be square and same size.);// ---- n 1 ----if(n1){doubledenA[0][0]*A[0][0]-1.0;if(std::fabs(den)1e-14)throwstd::runtime_error(dlyap(): solution not unique.);returnMatrix(1,Vector(1,-Q[0][0]/den));}// ---- n 2直接展开为 4x4 线性系统 ----if(n2){doubleaA[0][0],bA[0][1],cA[1][0],dA[1][1];MatrixM(4,Vector(4,0.0));Vectorrhs(4,0.0);M[0][0]a*a-1.0;M[0][1]a*b;M[0][2]b*a;M[0][3]b*b;rhs[0]-Q[0][0];M[1][0]c*a;M[1][1]d*a-1.0;M[1][2]c*b;M[1][3]d*b;rhs[1]-Q[1][0];M[2][0]a*c;M[2][1]b*c;M[2][2]a*d-1.0;M[2][3]b*d;rhs[2]-Q[0][1];M[3][0]c*c;M[3][1]c*d;M[3][2]d*c;M[3][3]d*d-1.0;rhs[3]-Q[1][1];Vector solsolveLinear(M,rhs);MatrixX(2,Vector(2));X[0][0]sol[0];X[1][0]sol[1];X[0][1]sol[2];X[1][1]sol[3];returnX;}// ---- n 2Kronecker 积向量化 ----// vec(A X A^T) (A ⊗ A) vec(X) → (A⊗A - I) vec(X) -vec(Q)constintn2n*n;MatrixM(n2,Vector(n2,0.0));Vectorrhs(n2,0.0);for(inti0;in;i)for(intj0;jn;j){introwi*nj;for(intp0;pn;p)for(intq0;qn;q){intcolp*nq;M[row][col]A[i][p]*A[j][q];if(piqj)M[row][col]-1.0;}rhs[row]-Q[i][j];}Vector vecXsolveLinear(M,rhs);MatrixX(n,Vector(n));for(inti0;in;i)for(intj0;jn;j)X[i][j]vecX[i*nj];// 对称化for(inti0;in;i)for(intji1;jn;j){doubleavg0.5*(X[i][j]X[j][i]);X[i][j]X[j][i]avg;}returnX;}// ---------------- main ----------------intmain(){Matrix A{{0.5,0.1},{0.0,0.3}};Matrix Q{{1.0,0.0},{0.0,1.0}};try{Matrix Xdlyap(A,Q);// 计算残差 A*X*A^T - X QMatrix AXmatmul(A,X);Matrix AXATmatmul(AX,transpose(A));doubleresnorm0.0;for(inti0;i2;i)for(intj0;j2;j){doublerAXAT[i][j]-X[i][j]Q[i][j];resnormr*r;}resnormstd::sqrt(resnorm);std::coutstd::fixedstd::setprecision(6);std::cout解 X:\n;for(constautorow:X){std::cout ;for(doublev:row)std::coutstd::setw(10)v ;std::cout\n;}std::cout残差范数: std::scientificstd::setprecision(2)resnorm\n;Vector eigsjacobiEigenvalues(X);std::coutstd::fixedstd::setprecision(5);std::coutX 的特征值:;for(doublee:eigs)std::cout e;std::cout\n;}catch(conststd::exceptione){std::cerrError: e.what()\n;return1;}return0;}