// lqry.cpp// 独立编译命令不依赖任何第三方库// g -stdc17 -O2 lqry.cpp -o lqry// ./lqry#includeiostream#includevector#includecmath#includecomplex#includeiomanip#includestdexcept#includestring#includealgorithm// // 自实现矩阵类替代 Eigen// classMatrix{public:introws,cols;std::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc):rows(r),cols(c),data(r*c,0.0){}doubleoperator()(inti,intj){returndata[i*colsj];}doubleoperator()(inti,intj)const{returndata[i*colsj];}staticMatrixidentity(intn){MatrixI(n,n);for(inti0;in;i)I(i,i)1.0;returnI;}Matrixtranspose()const{MatrixT(cols,rows);for(inti0;irows;i)for(intj0;jcols;j)T(j,i)(*this)(i,j);returnT;}Matrixoperator(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]o.data[i];returnR;}Matrixoperator-(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]-o.data[i];returnR;}Matrixoperator*(constMatrixo)const{MatrixR(rows,o.cols);for(inti0;irows;i)for(intk0;kcols;k){doubleaik(*this)(i,k);if(aik0.0)continue;for(intj0;jo.cols;j)R(i,j)aik*o(k,j);}returnR;}Matrixoperator*(doubles)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]*s;returnR;}doublemaxAbsDiff(constMatrixo)const{doublem0.0;for(size_t i0;idata.size();i)mstd::max(m,std::fabs(data[i]-o.data[i]));returnm;}// 高斯-约当消元法求逆Matrixinverse()const{intnrows;Matrix A*this;Matrix Iidentity(n);for(intcol0;coln;col){intpivcol;doublemvstd::fabs(A(col,col));for(intrcol1;rn;r)if(std::fabs(A(r,col))mv){mvstd::fabs(A(r,col));pivr;}if(mv1e-15)throwstd::runtime_error(矩阵奇异无法求逆);if(piv!col)for(intj0;jn;j){std::swap(A(col,j),A(piv,j));std::swap(I(col,j),I(piv,j));}doublepA(col,col);for(intj0;jn;j){A(col,j)/p;I(col,j)/p;}for(intr0;rn;r){if(rcol)continue;doublefA(r,col);if(f0.0)continue;for(intj0;jn;j){A(r,j)-f*A(col,j);I(r,j)-f*I(col,j);}}}returnI;}voidprint(conststd::stringname)const{std::coutname [rowsxcols]:\n;std::coutstd::setprecision(6);for(inti0;irows;i){std::cout ;for(intj0;jcols;j)std::coutstd::setw(12)(*this)(i,j) ;std::cout\n;}}};// // 2x2 矩阵的解析特征值求解// std::vectorstd::complexdoubleeigenvalues2x2(doublea,doubleb,doublec,doubled){std::vectorstd::complexdoubleev;doubletrad;doubledeta*d-b*c;doubledisctr*tr-4*det;if(disc0){doublesstd::sqrt(disc);ev.push_back({(trs)/2,0.0});ev.push_back({(tr-s)/2,0.0});}else{doublesstd::sqrt(-disc);ev.push_back({tr/2,s/2});ev.push_back({tr/2,-s/2});}returnev;}// // Riccati 方程迭代求解器// 求解: A^T P P A - (P B N) R^{-1} (B^T P N^T) Q 0// boolsolveRiccatiIterationC(constMatrixA,constMatrixB,constMatrixQ,constMatrixR,constMatrixN,MatrixP,doubledt0.001,doubletolerance1e-9,unsignedintiter_max200000){intnA.rows;PQ;// 初值 P QMatrixP_next(n,n);Matrix ATA.transpose();Matrix RinvR.inverse();for(unsignedinti0;iiter_max;i){Matrix PB_NP*BN;Matrix termAT*PP*A-PB_N*Rinv*PB_N.transpose()Q;P_nextPterm*dt;doublediffP_next.maxAbsDiff(P);PP_next;if(difftolerance){std::coutRiccati 迭代收敛迭代次数: istd::endl;returntrue;}}std::cerrRiccati 迭代未收敛std::endl;returnfalse;}// // lqry输出加权 LQR 设计// 最小化 J ∫ (y^T Q y u^T R u 2 y^T N u) dt, y Cx Du// boollqry(constMatrixA,constMatrixB,constMatrixC,constMatrixD,constMatrixQ,constMatrixR,MatrixK,MatrixS,std::vectorstd::complexdoublee,constMatrix*Nnullptr){intnA.rows,mB.cols,pC.rows;// 维度检查if(A.cols!n||B.rows!n){std::cerr错误: A 或 B 的维度不匹配std::endl;returnfalse;}if(C.cols!n||D.rows!p||D.cols!m){std::cerr错误: C 或 D 的维度不匹配std::endl;returnfalse;}if(Q.rows!p||Q.cols!p){std::cerr错误: Q 必须为 pxp 矩阵std::endl;returnfalse;}if(R.rows!m||R.cols!m){std::cerr错误: R 必须为 mxm 矩阵std::endl;returnfalse;}Matrix Nmat(N!nullptr)?*N:Matrix(p,m);// 输出加权 → 等效状态加权Matrix CtC.transpose();Matrix DtD.transpose();Matrix Q_barCt*Q*C;// C^T Q CMatrix R_barRDt*Q*DDt*NmatNmat.transpose()*D;// R D^T Q D D^T N N^T DMatrix N_barCt*Q*DCt*Nmat;// C^T Q D C^T N// 求解 Riccati 方程if(!solveRiccatiIterationC(A,B,Q_bar,R_bar,N_bar,S))returnfalse;// 最优增益 K R_bar^{-1} (B^T S N_bar^T)KR_bar.inverse()*(B.transpose()*SN_bar.transpose());// 闭环特征值Matrix A_clA-B*K;if(n!2){std::cerr本示例仅支持 n2 的特征值计算std::endl;returnfalse;}eeigenvalues2x2(A_cl(0,0),A_cl(0,1),A_cl(1,0),A_cl(1,1));returntrue;}// // 主函数// intmain(){// ---------- 系统定义 ----------MatrixA(2,2);A(0,0)0.6;A(0,1)0.25;A(1,0)0.0;A(1,1)0.9;MatrixB(2,1);B(0,0)0.0;B(1,0)10.0;MatrixC(1,2);C(0,0)11.0;C(0,1)0.0;MatrixD(1,1);D(0,0)0.0;MatrixQ(1,1);Q(0,0)2.0;MatrixR(1,1);R(0,0)1.0;// ---------- 调用 lqry ----------Matrix K,S;std::vectorstd::complexdoublee;booloklqry(A,B,C,D,Q,R,K,S,e);if(ok){std::cout\n lqry 求解结果 \n\n;K.print(最优增益 K);std::cout\n;S.print(Riccati 解 S);std::cout\n闭环特征值 e:\n;for(autoev:e)std::cout (ev.real(), ev.imag())\n;std::cout\n闭环矩阵 A - B*K 的特征值实部:\n;for(autoev:e)std::cout ev.real()\n;}else{std::cerrlqry 求解失败std::endl;}return0;}
C++代码实现MATLAB中的lqry函数功能
// lqry.cpp// 独立编译命令不依赖任何第三方库// g -stdc17 -O2 lqry.cpp -o lqry// ./lqry#includeiostream#includevector#includecmath#includecomplex#includeiomanip#includestdexcept#includestring#includealgorithm// // 自实现矩阵类替代 Eigen// classMatrix{public:introws,cols;std::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc):rows(r),cols(c),data(r*c,0.0){}doubleoperator()(inti,intj){returndata[i*colsj];}doubleoperator()(inti,intj)const{returndata[i*colsj];}staticMatrixidentity(intn){MatrixI(n,n);for(inti0;in;i)I(i,i)1.0;returnI;}Matrixtranspose()const{MatrixT(cols,rows);for(inti0;irows;i)for(intj0;jcols;j)T(j,i)(*this)(i,j);returnT;}Matrixoperator(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]o.data[i];returnR;}Matrixoperator-(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]-o.data[i];returnR;}Matrixoperator*(constMatrixo)const{MatrixR(rows,o.cols);for(inti0;irows;i)for(intk0;kcols;k){doubleaik(*this)(i,k);if(aik0.0)continue;for(intj0;jo.cols;j)R(i,j)aik*o(k,j);}returnR;}Matrixoperator*(doubles)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]*s;returnR;}doublemaxAbsDiff(constMatrixo)const{doublem0.0;for(size_t i0;idata.size();i)mstd::max(m,std::fabs(data[i]-o.data[i]));returnm;}// 高斯-约当消元法求逆Matrixinverse()const{intnrows;Matrix A*this;Matrix Iidentity(n);for(intcol0;coln;col){intpivcol;doublemvstd::fabs(A(col,col));for(intrcol1;rn;r)if(std::fabs(A(r,col))mv){mvstd::fabs(A(r,col));pivr;}if(mv1e-15)throwstd::runtime_error(矩阵奇异无法求逆);if(piv!col)for(intj0;jn;j){std::swap(A(col,j),A(piv,j));std::swap(I(col,j),I(piv,j));}doublepA(col,col);for(intj0;jn;j){A(col,j)/p;I(col,j)/p;}for(intr0;rn;r){if(rcol)continue;doublefA(r,col);if(f0.0)continue;for(intj0;jn;j){A(r,j)-f*A(col,j);I(r,j)-f*I(col,j);}}}returnI;}voidprint(conststd::stringname)const{std::coutname [rowsxcols]:\n;std::coutstd::setprecision(6);for(inti0;irows;i){std::cout ;for(intj0;jcols;j)std::coutstd::setw(12)(*this)(i,j) ;std::cout\n;}}};// // 2x2 矩阵的解析特征值求解// std::vectorstd::complexdoubleeigenvalues2x2(doublea,doubleb,doublec,doubled){std::vectorstd::complexdoubleev;doubletrad;doubledeta*d-b*c;doubledisctr*tr-4*det;if(disc0){doublesstd::sqrt(disc);ev.push_back({(trs)/2,0.0});ev.push_back({(tr-s)/2,0.0});}else{doublesstd::sqrt(-disc);ev.push_back({tr/2,s/2});ev.push_back({tr/2,-s/2});}returnev;}// // Riccati 方程迭代求解器// 求解: A^T P P A - (P B N) R^{-1} (B^T P N^T) Q 0// boolsolveRiccatiIterationC(constMatrixA,constMatrixB,constMatrixQ,constMatrixR,constMatrixN,MatrixP,doubledt0.001,doubletolerance1e-9,unsignedintiter_max200000){intnA.rows;PQ;// 初值 P QMatrixP_next(n,n);Matrix ATA.transpose();Matrix RinvR.inverse();for(unsignedinti0;iiter_max;i){Matrix PB_NP*BN;Matrix termAT*PP*A-PB_N*Rinv*PB_N.transpose()Q;P_nextPterm*dt;doublediffP_next.maxAbsDiff(P);PP_next;if(difftolerance){std::coutRiccati 迭代收敛迭代次数: istd::endl;returntrue;}}std::cerrRiccati 迭代未收敛std::endl;returnfalse;}// // lqry输出加权 LQR 设计// 最小化 J ∫ (y^T Q y u^T R u 2 y^T N u) dt, y Cx Du// boollqry(constMatrixA,constMatrixB,constMatrixC,constMatrixD,constMatrixQ,constMatrixR,MatrixK,MatrixS,std::vectorstd::complexdoublee,constMatrix*Nnullptr){intnA.rows,mB.cols,pC.rows;// 维度检查if(A.cols!n||B.rows!n){std::cerr错误: A 或 B 的维度不匹配std::endl;returnfalse;}if(C.cols!n||D.rows!p||D.cols!m){std::cerr错误: C 或 D 的维度不匹配std::endl;returnfalse;}if(Q.rows!p||Q.cols!p){std::cerr错误: Q 必须为 pxp 矩阵std::endl;returnfalse;}if(R.rows!m||R.cols!m){std::cerr错误: R 必须为 mxm 矩阵std::endl;returnfalse;}Matrix Nmat(N!nullptr)?*N:Matrix(p,m);// 输出加权 → 等效状态加权Matrix CtC.transpose();Matrix DtD.transpose();Matrix Q_barCt*Q*C;// C^T Q CMatrix R_barRDt*Q*DDt*NmatNmat.transpose()*D;// R D^T Q D D^T N N^T DMatrix N_barCt*Q*DCt*Nmat;// C^T Q D C^T N// 求解 Riccati 方程if(!solveRiccatiIterationC(A,B,Q_bar,R_bar,N_bar,S))returnfalse;// 最优增益 K R_bar^{-1} (B^T S N_bar^T)KR_bar.inverse()*(B.transpose()*SN_bar.transpose());// 闭环特征值Matrix A_clA-B*K;if(n!2){std::cerr本示例仅支持 n2 的特征值计算std::endl;returnfalse;}eeigenvalues2x2(A_cl(0,0),A_cl(0,1),A_cl(1,0),A_cl(1,1));returntrue;}// // 主函数// intmain(){// ---------- 系统定义 ----------MatrixA(2,2);A(0,0)0.6;A(0,1)0.25;A(1,0)0.0;A(1,1)0.9;MatrixB(2,1);B(0,0)0.0;B(1,0)10.0;MatrixC(1,2);C(0,0)11.0;C(0,1)0.0;MatrixD(1,1);D(0,0)0.0;MatrixQ(1,1);Q(0,0)2.0;MatrixR(1,1);R(0,0)1.0;// ---------- 调用 lqry ----------Matrix K,S;std::vectorstd::complexdoublee;booloklqry(A,B,C,D,Q,R,K,S,e);if(ok){std::cout\n lqry 求解结果 \n\n;K.print(最优增益 K);std::cout\n;S.print(Riccati 解 S);std::cout\n闭环特征值 e:\n;for(autoev:e)std::cout (ev.real(), ev.imag())\n;std::cout\n闭环矩阵 A - B*K 的特征值实部:\n;for(autoev:e)std::cout ev.real()\n;}else{std::cerrlqry 求解失败std::endl;}return0;}
