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

#includeiostream#includeiomanip#includevector#includecassert// 自实现的极简矩阵库 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];}// 矩阵乘法Matrixoperator*(constMatrixo)const{assert(colso.rows);Matrixres(rows,o.cols);for(inti0;irows;i)for(intk0;kcols;k){doubleadata[i*colsk];if(a0.0)continue;for(intj0;jo.cols;j)res(i,j)a*o(k,j);}returnres;}Matrixoperator(constMatrixo)const{assert(rowso.rowscolso.cols);Matrixres(rows,cols);for(size_t i0;idata.size();i)res.data[i]data[i]o.data[i];returnres;}Matrixoperator-(constMatrixo)const{assert(rowso.rowscolso.cols);Matrixres(rows,cols);for(size_t i0;idata.size();i)res.data[i]data[i]-o.data[i];returnres;}Matrixoperator*(doubles)const{Matrixres(rows,cols);for(size_t i0;idata.size();i)res.data[i]data[i]*s;returnres;}Matrixoperator(constMatrixo){for(size_t i0;idata.size();i)data[i]o.data[i];return*this;}};// LQG 调节器 // 对应 MATLAB: rlqg lqgreg(kest, k)// A_reg A - L*C - (B - L*D)*K// B_reg L// u -K * x_hatclassLQGRegulator{public:LQGRegulator(constMatrixA,constMatrixB,constMatrixC,constMatrixD,constMatrixL,constMatrixK):K_(K),n_(A.rows){A_reg_A-L*C-(B-L*D)*K;B_reg_L;x_hat_Matrix(n_,1);// 初始状态估计为零向量}// 前向欧拉更新返回控制量 u -K * x_hatMatrixupdate(constMatrixy,doubledt){Matrix dx_hatA_reg_*x_hat_B_reg_*y;x_hat_dx_hat*dt;returnK_*x_hat_*(-1.0);}constMatrixstateEstimate()const{returnx_hat_;}constMatrixregulatorA()const{returnA_reg_;}constMatrixregulatorB()const{returnB_reg_;}private:Matrix A_reg_,B_reg_,K_,x_hat_;intn_;};// 主程序 intmain(){// 示例二阶系统MatrixA(2,2);A(0,0)0;A(0,1)1;A(1,0)-2;A(1,1)-3;MatrixB(2,1);B(0,0)0;B(1,0)1;MatrixC(1,2);C(0,0)1;C(0,1)0;MatrixD(1,1);D(0,0)0;MatrixL(2,1);L(0,0)1.2;L(1,0)0.8;MatrixK(1,2);K(0,0)2.5;K(0,1)1.0;LQGRegulatorlqg(A,B,C,D,L,K);doubledt0.01;Matrixy(1,1);y(0,0)0.5;std::coutstd::fixedstd::setprecision(6);for(inti0;i10;i){Matrix ulqg.update(y,dt);constMatrixxhlqg.stateEstimate();std::coutStep i x_hat xh(0,0) xh(1,0) u u(0,0)std::endl;}return0;}