// dlqr.cpp// 独立实现 MATLAB 的 dlqr离散时间 LQR功能无需任何第三方库#includeiostream#includevector#includecmath#includeiomanip#includestring#includealgorithm#includestdexcept// 矩阵类 classMatrix{public:introws,cols;std::vectorstd::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc):rows(r),cols(c),data(r,std::vectordouble(c,0.0)){}staticMatrixIdentity(intn){MatrixI(n,n);for(inti0;in;i)I.data[i][i]1.0;returnI;}doubleoperator()(inti,intj){returndata[i][j];}constdoubleoperator()(inti,intj)const{returndata[i][j];}// 矩阵加法Matrixoperator(constMatrixo)const{Matrixr(rows,cols);for(inti0;irows;i)for(intj0;jcols;j)r.data[i][j]data[i][j]o.data[i][j];returnr;}// 矩阵减法Matrixoperator-(constMatrixo)const{Matrixr(rows,cols);for(inti0;irows;i)for(intj0;jcols;j)r.data[i][j]data[i][j]-o.data[i][j];returnr;}// 矩阵乘法Matrixoperator*(constMatrixo)const{if(cols!o.rows)throwstd::runtime_error(Matrix multiply: dimension mismatch);Matrixr(rows,o.cols);for(inti0;irows;i)for(intk0;kcols;k){doubleadata[i][k];if(a0.0)continue;for(intj0;jo.cols;j)r.data[i][j]a*o.data[k][j];}returnr;}// 转置Matrixtranspose()const{Matrixr(cols,rows);for(inti0;irows;i)for(intj0;jcols;j)r.data[j][i]data[i][j];returnr;}// 用 Cholesky 分解求解对称正定线性方程组 A*X BMatrixsolveCholesky(constMatrixB)const{intnrows;if(n!cols||B.rows!n)throwstd::runtime_error(solveCholesky: dimension mismatch);// 计算下三角 L使得 A L * L^TMatrixL(n,n);for(inti0;in;i){for(intj0;ji;j){doublesdata[i][j];for(intk0;kj;k)s-L.data[i][k]*L.data[j][k];if(ij){if(s0.0)throwstd::runtime_error(Cholesky: matrix not positive definite);L.data[i][j]std::sqrt(s);}else{L.data[i][j]s/L.data[j][j];}}}// 前代求解 L * Y BMatrixY(n,B.cols);for(intj0;jB.cols;j)for(inti0;in;i){doublesB.data[i][j];for(intk0;ki;k)s-L.data[i][k]*Y.data[k][j];Y.data[i][j]s/L.data[i][i];}// 回代求解 L^T * X YMatrixX(n,B.cols);for(intj0;jB.cols;j)for(intin-1;i0;--i){doublesY.data[i][j];for(intki1;kn;k)s-L.data[k][i]*X.data[k][j];X.data[i][j]s/L.data[i][i];}returnX;}// 与另一矩阵逐元素最大绝对差doublemaxAbsDiff(constMatrixo)const{doublem0.0;for(inti0;irows;i)for(intj0;jcols;j)mstd::max(m,std::abs(data[i][j]-o.data[i][j]));returnm;}};// 打印 voidprintMatrix(conststd::stringname,constMatrixM){std::coutname:\n;for(inti0;iM.rows;i){for(intj0;jM.cols;j)std::coutstd::setw(12)std::fixedstd::setprecision(6)M.data[i][j];std::cout\n;}}// DLQR 求解 booldlqr(constMatrixA,constMatrixB,constMatrixQ,constMatrixR,MatrixK,MatrixP,doubletol1e-8,unsignedintmax_iter10000){intnA.rows;intmB.cols;PQ;MatrixP_next(n,n);Matrix AdTA.transpose();Matrix BdTB.transpose();for(unsignedintiter0;itermax_iter;iter){// DARE 迭代 P - A^T P A - A^T P B (R B^T P B)^{-1} B^T P A QMatrix R_plus_BPBRBdT*P*B;Matrix solR_plus_BPB.solveCholesky(BdT*P*A);P_nextAdT*P*A-AdT*P*B*solQ;doublediffP_next.maxAbsDiff(P);PP_next;if(difftol){// 计算反馈增益 K (B^T P B R)^{-1} B^T P AMatrix R_plus_BPB_finalRBdT*P*B;KR_plus_BPB_final.solveCholesky(BdT*P*A);returntrue;}}std::cerrdlqr: 达到最大迭代次数仍未收敛std::endl;returnfalse;}// 2x2 特征值 voideigenvalues2x2(constMatrixM,doublere1,doubleim1,doublere2,doubleim2){doubleaM(0,0),bM(0,1),cM(1,0),dM(1,1);doubletrad;doubledeta*d-b*c;doubledisctr*tr-4.0*det;if(disc0.0){doublesstd::sqrt(disc);re1(trs)/2.0;im10.0;re2(tr-s)/2.0;im20.0;}else{doublesstd::sqrt(-disc);re1tr/2.0;im1s/2.0;re2tr/2.0;im2-s/2.0;}}// 主程序 intmain(){// 二阶示例系统MatrixA(2,2);A(0,0)1.0;A(0,1)0.1;A(1,0)0.0;A(1,1)0.9;MatrixB(2,1);B(0,0)0.0;B(1,0)0.1;Matrix QMatrix::Identity(2);Q(0,0)10.0;Q(1,1)10.0;MatrixR(1,1);R(0,0)0.1;Matrix K,P;if(dlqr(A,B,Q,R,K,P)){printMatrix(反馈增益 K,K);printMatrix(Riccati 方程解 P,P);// 闭环极点Matrix A_clA-B*K;doublere1,im1,re2,im2;eigenvalues2x2(A_cl,re1,im1,re2,im2);std::cout闭环极点:\n;std::cout (std::fixedstd::setprecision(5)re1,im1)\n;std::cout (std::fixedstd::setprecision(5)re2,im2)\n;}return0;}