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

#include<iostream>#include<iomanip>#include<vector>#include<complex>#include<algorithm>#include<cmath>#include<stdexcept>usingComplex=std::complex<double>;// ===================== 多项式求值（Horner 法） =====================// 实系数多项式，按降幂排列，求 P(s)Complexpolyval(conststd::vector<double>&p,Complex s){Complexval(0.0,0.0);for(doublec:p)val=val*s+c;returnval;}// 复系数多项式，按降幂排列，求 P(s)Complexpolyval(conststd::vector<Complex>&p,Complex s){Complexval(0.0,0.0);for(constauto&c:p)val=val*s+c;returnval;}// ===================== Durand-Kerner 多项式求根 =====================// 输入：实系数多项式 coeff（降幂排列），coeff[0] != 0// 输出：全部复根（个数 = 多项式次数）std::vector<Complex>poly_roots(conststd::vector<double>&coeff){intn=static_cast<int>(coeff.size())-1;std::vector<Complex>roots(n);if(n<=0)returnroots;if(n==1){roots[0]=Complex(-coeff[1]/coeff[0],0.0);returnroots;}// 归一化为首一多项式std::vector<Complex>a(n+1);for(inti=0;i<=n;++i)a[i]=Complex(coeff[i]/coeff[0],0.0);// 初始猜测 roots[k] = seed^k, k = 0..n-1Complexseed(0.4,0.9);roots[0]=Complex(1.0,0.0);for(intk=1;k<n;++k)roots[k]=roots[k-1]*seed;constdoubletol=1e-15;constintmax_iter=2000;std::vector<Complex>new_roots(n);for(intiter=0;iter<max_iter;++iter){doublemax_delta=0.0;for(intk=0;k<n;++k){Complex p_val=polyval(a,roots[k]);Complexdenom(1.0,0.0);for(intj=0;j<n;++j)if(j!=k)denom*=(roots[k]-roots[j]);Complex delta=p_val/denom;new_roots[k]=roots[k]-delta;doubled=std::abs(delta);if(d>max_delta)max_delta=d;}roots=new_roots;if(max_delta<tol)break;}returnroots;}// ===================== rlocfind 核心 =====================structRlocfindResult{doublegain;// 反算出的增益 Kstd::vector<Complex>poles;// 该 K 下的所有闭环极点Complex selected_pole;// 离目标点最近的极点};RlocfindResultrlocfind(conststd::vector<double>&den,conststd::vector<double>&num,Complex target){Complex D_val=polyval(den,target);Complex N_val=polyval(num,target);if(std::abs(N_val)<1e-14)throwstd::runtime_error("在目标点处 N(s) ≈ 0，无法反解增益 K");doubleK=(-D_val/N_val).real();// 构造特征多项式 P(s) = D(s) + K * N(s)intn=static_cast<int>(den.size())-1;std::vector<double>num_pad(den.size(),0.0);intshift=static_cast<int>(den.size())-static_cast<int>(num.size());for(size_t i=0;i<num.size();++i)num_pad[i+shift]=num[i];std::vector<double>poly(n+1);for(inti=0;i<=n;++i)poly[i]=den[i]+K*num_pad[i];autopoles=poly_roots(poly);// 找出离目标点最近的极点Complex best=poles[0];doublebest_dist=std::abs(poles[0]-target);for(constauto&p:poles){doubled=std::abs(p-target);if(d<best_dist){best_dist=d;best=p;}}return{K,poles,best};}// ===================== main =====================intmain(){std::cout<<std::setprecision(6);// 开环传递函数 G(s) = 1 / [s(s+1)(s+2)]// 分母：den = s^3 + 3s^2 + 2s// 分子：num = 1std::vector<double>num={1.0};std::vector<double>den={1.0,3.0,2.0,0.0};// 用户“点击”的期望闭环极点Complextarget(-0.38,0.36);try{autores=rlocfind(den,num,target);// 按实部降序、实部相同按虚部降序排序，使输出顺序稳定autosorted=res.poles;std::sort(sorted.begin(),sorted.end(),[](constComplex&a,constComplex&b){if(std::abs(a.real()-b.real())>1e-9)returna.real()>b.real();returna.imag()>b.imag();});std::cout<<"反算增益 K = "<<res.gain<<std::endl;std::cout<<"该 K 下的全部闭环极点："<<std::endl;for(constauto&p:sorted)std::cout<<" "<<p<<std::endl;std::cout<<"离目标最近的极点："<<res.selected_pole<<std::endl;}catch(conststd::exception&e){std::cerr<<"错误："<<e.what()<<std::endl;}return0;}