%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % mainRic.m solves "general" LQR problem % % % % % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global ti tf Tper global A B C Q0 Qref R S global p tp % The System matrices A=[-0.089 -2.19 0 0.319 0 0 ; 0.076 -0.217 -0.166 0 0 0 ;-0.602 0.327 -0.975 0 0 0 ;0 0.15 1 0 0 0 ;0 1 0 0 0 0 ;1 0 0 0 2.19 0] B=[0 0.0327 ; 0.0264 -0.151 ; 0.227 0.0636 ; 0 0 ; 0 0 ; 0 0] C=[0 0 0 0 0 1]; % The Cost functional Q0=eye(6).*500; xTref=[0 0 0 0 0 10]'; g0=-Q0*xTref; Qref=20 R=eye(2).*1; S=zeros(6,2); % Start and final time ti=0; tf=10; % boundary values p1=PtoX(Q0,g0,0); x0=zeros(6,1); % Integrate the Riccati equations [tp,p] = ode45('fRic',[ti tf],[p1]); % Integrate the system equations [tx,x] = ode45('fSys',[ti tf],[x0]); clear u % ReCalculate the control (for plotting) for i=1:length(x) t=tx(i); tRev=tf-(t-ti); pRicc_t=(interp1(tp,p,tRev))'; n=length(A); [P,q,c]=XtoP(pRicc_t,n); x_t=x(i,:)'; Vx=2*(P*x_t+q); u_t=(-1/2)*inv(R)*(B'*Vx+2*S'*x_t); u(:,i)=u_t; end subplot(3,1,1) plot(tx,x(:,6),'-.',tx,5*(1-cos(1*pi*tx/tf)),'-') ylabel('feet') legend('y','ref') subplot(3,1,2) plot(tx,u(1,:),'-.',tx,u(2,:),'-') ylabel('rad*10^{-2}') legend('da','dr') subplot(3,1,3) plot(tx,x(:,4),'-.',tx,x(:,5),'-') ylabel('rad*10^{-2}') xlabel('time (s)') legend('Roll','Yaw')