自适应控制Matlab实现例子

close all; clear all; %Neural network based control of an inverted pendulum % %Author Girish Chowdhary %date: 18/oct/06 %Code simulates the control of an inverted pendulum with PCH and NN based %model reference control, %Distributed with Adaptive Control workshop at the 50th CDC %% globals global RESIDUAL_REC a bw n1 n2 n3 gammaV gammaW_Full B P back_flag Wc Vc index WdotStore VdotStore mom VAD_REC X_REC XDOT_REC qmod_win bv qmod_coeff delta_LM Kalr old_res gammaV_Full global deltaErrS beta global e_mjwt omega RFT_rr X deltaErr theta WdotStore=0; VdotStore=0; %% %% main sim control CL_on=1; %set to 0 if no concurrent learning, to 1 to have concurrent learning %% integration parameters %integration parameters t0=0; tf=150 dt=0.05; %% reference model parameters omegan_rm = 1; % reference model natural freq (rad/s) zeta_rm = 1; % reference model damping ratio %% command array xcmd = [0:dt:tf]*0; % commanded position (rad) stepup=dt; step=20; stepup1=step; stepdown=2*step; xcmd(1/dt:10/dt)=0; xcmd(10/dt:30/dt)=1; xcmd(50/dt:70/dt)=1; xcmd(90/dt:110/dt)=1; xcmd(130/dt:150/dt)=1; %% control parameters %control parameters controlUpdateDT = 0.02; % controller update rate Kp = 2.1; % proportional gain Kd = 2.2; % derivative gain %% Neural network parameters n1=2; %n1 no of states(inputs to NN) n2=8; %n2 no of hidden nodes (neurons) n3=1; %n3 no of output layer nodes\ amin = 0.01; amax = 5; a = zeros(n2,1); % assign activation potential (different for all nodes, zero for the first) for ii=1:n2, a(ii) = tan( atan(amin) + ( atan(amax) - atan(amin) )*(ii)/n2 ); end gammaW=3; %W learning rate %3 gammaV=1; %V learning rate%1 kappa=0.0; %NN damping mom=0.00; %momentum term Kalr=0.000; %Adaptive Loop Recovery Term%off time default bv=1; bw=1; deltaErr=0; %% Initialization % solve Lyapunov equation A=[0 1;-Kp -Kd]; B = [0; 1]; Q = eye(2); P = lyap(A',Q);%A' instead of A because need to solve A'P+PA+Q=0 [Lp,PP,E] = lqr(A,B,eye(2)*100,1); % initialize states x1 = 0; x2 = 0; xddot =[0; 0]; x1_rm = x1; %x1 reference model x2_rm = x2; %x2 reference model x = [x1;x2]; xn=x; edot1=[0;0]; epsilon=0; e1=edot1; x_rm = x; V = zeros( (n1+1), n2 );%NN input matrix W = zeros( (n2+1), n3 );%NN hidden matrix %% for separate integration Wt = zeros( (n2+1), n3 );%NN hidden matrix Wb = zeros( (n2+1), n3 );%NN hidden matrix Vt=V*0;Vb=V*0; WW=W; %% % initialize control v_crm = 0; %v reference model v_pd = 0; %v proportional-derivative controller v_ad = 0; %v adaptive (NN) v_h = 0; %V PCH delta = 0; %command initilization deltaCmd = 0; %PCh cmd deltaLim = 0.1; % control limit delta_i=0; deltaErrHat=0; lastControlUpdate = 0; %control update flag %% Background learning parameter initialization max_back_points=5; xback=[0; 0; 0;]; %background learning vector eback=[0;0]; residual=zeros(1,max_back_points); rresidual=0; deltaErrS=0; pp=1; %background learning data point index (pointer to last update) no_back_points=0; %no. of background learning points back_point_selector=1; zz=pp; SIGMADASH=0; temp=0; back_flag=1; beta=0; gammaWb=ones(1,max_back_points)*gammaW*1/3; %learning rate for background learning W matrix adaptation gammaVb=ones(1,max_back_points)*gammaV*3; %learning rate for background learning V matrix adaptation if CL_on==1 gammaW_Full=[gammaW, gammaWb]; %these will be sent to W V int gammaV_Full=[gammaV,gammaVb]; else gammaW_Full=[gammaW, gammaWb*0]; %these will be sent to W V int gammaV_Full=[gammaV,gammaVb*0]; end delta_LM=gammaVb; old_res=0; %% Qmod parameters qmod_win=10; qmod_coeff=0.0; %% Least squares parameter initialization freqs = [0:.02:5]; % analysis frequencies (low,increment,high) [Hz] omega = (freqs.*(2*pi))'; e_mjwt = ones(size(freqs))'; RFT_rr=0; X=zeros(max(size(freqs)),n2+1); theta=W*0; %% Smoothing parameter initialization ns=3; wn=0.02;%strength of gaussian random noise an=0.18;%frequency of sinusoidal noise rps=860*2*pi/60;%860 rpm in radians per second x2_hat=0; xdotdot_hat=0; x_hat=[0,0,0]';%[x,xdot,xdotdot]' F=expm(dt*[0,1,0;0,0,1;0,0,0]); H=[1 0 0;0 1 0];%y=[x,xdot]' Qk=diag([0 0 wn^2]); Rk=diag([1,1])*wn^2; PD=diag([wn^2 wn^2 wn^2]); pdm=F*PD*F'+Qk; x_hat_min=x_hat; xhat_hat=x_hat; xhat_hatS=x_hat; PknS=PD; NN=30; %One step storing variables xhat_hatSS=x_hat; PDS=PD; x_hatS=x_hat; x_hat_minS=x_hat; %backward filter parameters %% stability metric parameters Twin=100; lambda=0; %% Data recording intialization t=t0:dt:tf; T_REC = zeros(length(t),1); X_REC = zeros(length(t),1); X_HAT_REC = zeros(length(t),1); XHAT_minus_STORE=zeros(length(t),ns)'; XHAT_STORE = XHAT_minus_STORE; XDOT_REC = zeros(length(t),1); XDOT_HAT_REC = zeros(length(t),1); XDOTDOT_REC = zeros(length(t),1); XDOTDOT_HAT_REC = zeros(length(t),1); XHAT_HAT_REC = XHAT_minus_STORE; XRM_REC = zeros(length(t),1); XDOTRM_REC = zeros(length(t),1); XERR_REC = zeros(length(t),1); XDOTERR_REC = zeros(length(t),1); DELTACMD_REC = zeros(length(t),1); DELTA_REC = zeros(length(t),1); DELTAERR_REC = zeros(length(t),1); DELTAERR_HAT_REC= zeros(length(t),1); VCRM_REC = zeros(length(t),1); VAD_REC = zeros(length(t),1); VH_REC = zeros(length(t),1); VPD_REC = zeros(length(t),1); SIGMA_REC = zeros(length(t),1); V_REC = zeros((n1+1), n2,length(t)); W_REC = zeros((n2+1),length(t)); vv = zeros(length(t),3); STORE = zeros(length(t),1); RESIDUAL_REC = zeros(10,length(t)); EDOT_REC = zeros(n1,length(t)); COVARIANCE_STORE=zeros(ns,ns,length(t)); COVARIANCE_minus_STORE=zeros(ns,ns,length(t)); Kalman_Gain_Store=zeros(3,2,length(t)); BETA_store = zeros(length(t),1); Lambda_store=zeros(length(t),1); COUNTER_REC = zeros(length(t),1); %% Main simulation loop (convert to a function with sub functions) index=1; for t=t0:dt:tf %% compute reference model error e=x_rm-x;%compute reference model error %% If control? controller runs at a different update rate: controlDT xref =xcmd(index); controlDT=t-lastControlUpdate; %update reference model v_crm=omegan_rm^2*(xref-x_rm(1))-2*zeta_rm*omegan_rm*x_rm(2); v_h=deltaCmd-delta; %update NN xbar=[bv;x_hat(1);x_hat(2)]; %% Sigma Update z=V'*xbar;%([n2*(n1+1)]*(n1+1)*1=(n2)*1 [sigma,sigmadash]=sdash(z,a,bw,n2); v_ad = W'*sigma; %% adaptation input %% %%W, V update [W,V,r]=W_V_int(gammaW_Full,gammaV_Full,sigma,sigmadash,V,W,xbar,e,P,B,kappa,residual,xback,no_back_points,controlDT); %qMOD q=sigma*0; c=0; BETA_store(index)=beta; %% Update control v_pd=[Kp Kd]*e; deltaCmd = v_crm + v_pd - v_ad;%Nu lastControlUpdate = t; %% store data points for background learning if abs(xbar(1))+abs(xbar(2))+abs(xbar(3))>bv; %store only data points with %nonzero xbar(2) and xbar(3) %values since xbar(1) = bv=1 Ssize=size(xback); if (xbar-xback(:,pp))'*(xbar-xback(:,pp))/(xbar'*xbar)>=0.3; no_back_points=no_back_points+1; if no_back_points<=max_back_points pp=no_back_points; elseif no_back_points > max_back_points % pp=1; if back_point_