Hello,
I'm trying to solve the following linear quadratic tracking problem using Matlab.
The dynamics are given by: x-dot = Ax+Bu (scalar system)
The objective function is: J= 1/2 x(tf)^2 + 1/2 integral {x' Q x + u' R u} dt
(to=0 and tf=2)
The goal is to drive x(t) to a desired value xd, i.e., track a constant reference.
I used the algorithm (picture attached) in the book by Naidu (Optimal Control Systems)
and wrote Matlab code to implement it. The differential equations of the Ricatti equations
p-dot and g-dot are solved backwards, and they seem to be correct from the figures.
The output x(t), however, does not reach the target xd (I used xd=3 in the simulation).
I would appreciate any help with this matter.
The Matlab code is below:
I'm trying to solve the following linear quadratic tracking problem using Matlab.
The dynamics are given by: x-dot = Ax+Bu (scalar system)
The objective function is: J= 1/2 x(tf)^2 + 1/2 integral {x' Q x + u' R u} dt
(to=0 and tf=2)
The goal is to drive x(t) to a desired value xd, i.e., track a constant reference.
I used the algorithm (picture attached) in the book by Naidu (Optimal Control Systems)
and wrote Matlab code to implement it. The differential equations of the Ricatti equations
p-dot and g-dot are solved backwards, and they seem to be correct from the figures.
The output x(t), however, does not reach the target xd (I used xd=3 in the simulation).
I would appreciate any help with this matter.
The Matlab code is below:
Code:
clc
close all
clear all
% The differential eqn.
% x-dot = Ax+Bu
A = -1;
B = 1;
% objective function matrices (parameters)
Sf = 1;
Q = 1;
R = 1;
E = B*inv(R)*B';
C = 1;
W = C'*Q ;
V = C'*Q*C;
xd =3; % desired value for x
% Finding the dynamic equations for p-dot and g-dot
syms p
syms g
p_dot = -p*A - A'*p + p*E*p-V;
g_dot= -(A-E*p)'*g - W*xd;
% initial and final conditions
x0 = 0; % initial value for x(t)
pf = 1; % final value for p(t)
gf = Sf*xd; % final value for g(t)
t0 = 0;
tf = 2;
% solving backwards for the differential Riccati equations p-dot, g-dot
[t,z]=ode45(@lqr_pg, [tf t0], [pf;gf]);
% solving forward for the closed-loop state equations ( z=[p;g] )
for j = 1:length(t)
[t1,x]=ode45( @(t,x)sys(t, x, z(j,:)), [t0 tf], x0);
end
% initializing vector u= -Kx+R^-1 B' g(t) (K=R^-1 * B' * p)
u = zeros(length(t1), size(B,1));
for i = 1:length(t)
uu = -inv(R)*B'*z(i,1)*x(i)+ inv(R)*B'*z(i,2);
u(i) = uu;
end
% ploting vector p(t)
figure(1)
plot(t,z(:,1))
ylabel('p(t)')
xlabel('time')
% ploting vector g(t)
figure(2)
plot(t,z(:,2))
ylabel('g(t)')
xlabel('time')
grid
figure(3) % plotting output x(t)
plot(t1,x)
ylabel('x(t)')
xlabel('time')
figure(4) % plotting control u(t)
plot(t1,u)
ylabel('u(t)')
xlabel('time')
% solving p_dot and g_dot, z=[p g]'
function dz=lqr_pg(t,z)
A = -1;
B = 1;
R = 1;
W = 1;
E = B*inv(R)*B';
xd = 3; % desired value for x
z1dot = z(1)^2+2*z(1)-1; % z(1) is p
z2dot = -(A-E*z(1))'*z(2) - W*xd; % z(2) is g
dz = [z1dot; z2dot];
end
function xdot = sys(~, x, z)
A = -1;
B = 1;
R = 1;
E = B*inv(R)*B';
xdot = (A-E*z(:,1))*x+E*z(:,2);
end
