%%%%%%%%%%%%
clear all
A=[0. ,1. ;-2. ,1.] ; B= [ 0. ; 1.] ;
Q=[2. ,3. ;3. ,5.] ;
F=[1. ,0.5;0.5,2.] ;
R=[0.25] ;
tspan=[O 5];
xo=[2. ,-3.] ;
[x,u,K]=lqrnss(A,B,F,Q,R,xO,tspan);
%%%%%%%%%%%%%

%%%%%%%%%%%%%
%% The following is lqrnss.m
function [x,u,K]=lqrnss(As,Bs,Fs,Qs,Rs,xO,tspan)
%Revision Date 11/14/01
%%
%This m-file calculates and plots the outputs for a %Linear Quadratic Regulator (LQR) system based on given % state space matrices A and B and performance index
% matrices F, Q and R. This function takes these inputs, %and using the analytical solution to the
%% matrix Riccati equation,
%and then computing optimal states and controls.
%
%
% SYNTAX: [x,u,K]=lqrnss(A,B,F,Q,R,xO,tspan) %
% INPUTS (All numeric):
Matrices from xdot=Ax+Bu Performance Index Parameters;
State variable initial condition Vector containing time span [to tf]
% A,B
% F,Q,R
% xO
% tspan %
% OUTPUTS:
% x
% u
% K %
% The system plots Riccati coefficients, x vector, %and u vector
%
%Define variables to use in external functions
%
global A E F Md tf W11 W12 W21 W22 n,
%
%Check for correct number of inputs
%
if nargin<7
    error('Incorrect number of inputs specified')
    return
end
%
%Convert Variables to normal symbology to prevent %problems with global statement
%
A=As;
B=Bs;
F=Fs;
Q=Qs;
R=Rs;
plotflag = O;
%data on figures
%
%Define secondary variables for global passing to
% ode-related functions %
[n,m]=size(A);
[nb,mb]=size(B);
[nq,mq] =size (Q) ;
[nr,mr]=size(R);
[nf ,mf] =size (F) ;
if n~=m
    error('A must be square')
else
    [n, n] =size(A);
end
%
%Data Checks for proper setup
if length(A>rank(ctrb(A,B))
%Check for controllability
    error('System Not Controllable')
    return
end
if(n~=nq)|(n~=mq)
%Check that A and Q are the same size
    error('A and Qmust be the same size');
    return
end
if~(mf==l&nf==l)
    if(nq ~= nf)|(mq ~= mf)
%Check that Q and F are the same size
        error('Q and F must be the same size');
        return
    end
end
if ~(mr==l & nr==l)
    if(mr~=nr)|(mb~=nr)
    error('R must be consistent with B');
    return
    end
end
mq = norm(Q,l);
% Check if Q is positive semi-definite and symmetric
if any(eig(Q) < -eps*mq) I (norm(Q'-Q,l)/mq > eps)
    disp('Warning: Q is not symmetric and positive ... semi-definite');
end
mr = norm(R,1);
% Check if R is positive definite and symmetric
if any(eig(R) <= -eps*mr) I (norm(R'-R,l)/mr > eps)
    disp('Warning: R is not symmetric and positive ... definite');
end
%
%Define Initial Conditions for
%numerical solution of x states
%
to=tspan (1) ;
tf=tspan(2);
tspan=[tf to];
%
%Define Calculated Matrices and Vectors
%
E=B*inv(R)*B' ; %E Matrix E=B*(l/R)*B'
%
%Find Hamiltonian matrix needed to use %analytical solution to
%matrix Riccati differential equation %
Z=[A,-E;-Q,-A'];
%
%Find Eigenvectors
%
[W,DJ] = eig(Z) ;
%
%Find the diagonals from D and pick the %negative diagonals to create
%a new matrix M
%
j=n;
[ml,indexlJ=sort(real(diag(D)));
    for i=l:l:n
    m2(i)=ml(j);
    index2(i)=indexl(j);
    index2(i+n)=indexl(i+n);
    j=j-l;
    end
Md=-diag(m2);
%
%Rearrange W so that it corresponds to the sort %of the eigenvalues
%
for i=1:2*n
    w2(:,i)=W(:,index2(i));
end
W=w2;
%
%Define the Modal Matrix for D and Split it into Parts %
Wll=zeros(n);
W12=zeros(n);
W21=zeros(n);
W22=zeros(n);
j=l;
for i=1:2*n:(2*n*n-2*n+l)
    Wll(j:j+n-l)=W(i:i+n-l);
    W21(j:j+n-l)=W(i+n:i+2*n-l);
    W12(j:j+n-l)=W(2*n*n+i:2*n*n+i+n-l);
    W22(j:j+n-l)=W(2*n*n+i+n:2*n*n+i+2*n-l);
    j=j+n;
end %
%Define other initial conditions for % calculation of P, g, x and u
%
tl=O. ;
tx=O. ;
tu=O. ;
x=O. ;
%
%Calculation of optimized x %
%time array for x %time array for u %state vector
[tx,x]=ode45('lqrnssf',fliplr(tspan),xO, ...
    odeset('refine',2,'RelTol',le-4,'AbsTol',le-6));
%
%Find u vector
%
j=1;
us=O.; %Initialize computational variable
for i=l:l:mb
    for tua=tO:.l:tf
        Tt=-inv(W22-F*W12)*(W21-F*Wll);
        P=(W21+W22*expm(-Md*(tf-tua))*Tt* ...
expm(-Md*(tf-tua)))*inv(Wll+W12*expm(-Md*(tf-tua)) ...
    *Tt*expm(-Md*(tf-tua)));
    K=inv(R)*B'*P;
    xs=interpl(tx,x,tua);
    usl=real(-K*xs');
    us(j) =usl(i);
    tu(j)=tua;
    j=j+l;
    end
    u(:,i)=us' ;
    us=O;
    j=1;
end
%
%Provide final steady-state K
%
P = W21/W11;
K=real(inv(R)*B'*P);
%
%Plotting Section, if desired
%
if plotflag~=l
%
%Plot diagonal Riccati coefficients using a
%flag variable to hold and change colors %
fig=1 ; %Figure number
cflag=1; %Variable used to change plot color
j=1;
Ps=O. ;
for i=1:1:n*n %Initialize P matrix plot variable
    for tla=tO: .1:tf
    Tt=-inv(W22-F*W12)*(W21-F*Wll);
    P=real<W21+W22*expm(-Md*(tf-tla))*Tt*expm(-Md* ...
    (tf-tla)))*inv(Wll+W12*expm(-Md*(tf-tla))*Tt ...
    *expm(-Md*(tf-tla))));
    Ps(j)=P(i);
    tl(j)=t1a;
    j=j+l ;
    end
    if cflag==l;
    figure(fig)
    plot(t1,Ps, 'b')
    title('Plot of Riccati Coefficients')
    xlabel('t')
    ylabel ('P Matrix') .
    hold
    cflag=2;

    elseif cflag==2
        plot( t1, Ps,' m: ' )
        cflag=3;
    elseif cflag==3
        plot(t1,Ps,'g-.')
        cflag=4;
    elseif cflag==4
        plot(t1,Ps,'r--')
        cflag=1 ;
        fig=fig+1;
    end
Ps=O. ;
j=1 ;
end
    if cflag==2|cflag==3|cflag==4
    hold
    fig=fig+1;
    end
%
%Plot Optimized x %
if n>2
    for i=1:3:(3*fix((n-3)/3)+1)
        figure(fig);
        plot(tx,real(x(:,i)),'b',tx,real(x(:,i+1)),'m:',tx, ...
            real(x(:,i+2)),'g-.')
        title('Plot of Optimized x')
        xlabel('t')
        ylabel('x vectors')
        fig=fig+1;
    end
end
if (n-3*fix(n/3))==1
    figure(fig);
    plot(tx,real(x(:,n)),'b')
elseif (n-3*fix(n/3))==2
figure(fig);
plot( tx , real(x(:,n-1)),'b', tx , real(x(:,n)), 'm:' )
end
title('Plot of Optimized x')
xlabel('t')
ylabel('x vectors')
fig=fig+1;
%
%Plot Optimized u %
if mb>2
for i=1:3:(3*fix((mb-3)/3)+1)
    figure(fig);
    plot(tu,real(u(:,i)),'b',tu,real(u(:,i+1)),'m:', ...
        tu,real(u(:,i+2)),'g-.')
    title('Plot of Optimized u')
    xlabel('t')
    ylabel('u vectors')
    fig=fig+1;
%
    end
end
if (mb-3*fix(mb/3))==1
    figure(fig);
    plot(tu,real(u(:,mb)),'b')
elseif (mb-3*fix(mb/3))==2
    figure(fig);
    plot(tu,real(u(:,mb-1)),'b',tu,real(u(:,mb)),'m:')
end
title('Plot of Optimized u')
xlabel('t')
ylabel('u vectors')
%
end
%% %%%%%%%%%%%%%