LQR Implementation in MATLAB

I am trying to implement a simple LQR controller in MATLAB for a purely deterministic system. The code is shown below:

%% Continuous Time
clear all; close all; clc;
% Parameters
n = 2;
m = 1;
A = [1 1; 0 1];
B = [0.5; 1];
C = [1 0];
Q = eye(2);
QT = Q;
R = 10;
x0 = [1; 0];
T = 10;
N = 50;

% Backwards recursion for P(t), t = T -> 0
P0 = QT;
[tout,Pvecout] = ode45(@(t,Pvec) mRiccati(t,Pvec,A,B,Q,R,n),[0 T], P0(:));
Pvecout = flip(Pvecout);

% Flip (and interpolate) to get P(t), t = 0 -> T
t = linspace(0,T,N);
deltat = t(end) - t(end-1);
P = [];
for i = 1:N
Pvecoutk = interp1(tout,Pvecout,t(i));
P(:,:,i) = reshape(Pvecoutk,n,n);
end

% Simulate dynamics with (time-varying) LQR controller
x = x0;
xk = x0;
u = [];
% Linearized dynamics (!)
Ak = eye(2) + deltat * A;
Bk = deltat * B;
for i = 1:N-1
Kk = inv(R) * B' * P(:,:,i);
uk = -Kk * xk;
xk = Ak * xk + Bk * uk;
x = [x, xk];
u = [u, uk];
end

% Plot results
figure; hold on; grid on;
plot(t,x(1,:)); plot(t,x(2,:));
figure; hold on; grid on;
plot(t(1:end-1),u);

%% Useful Functions
function dPdtvec = mRiccati(t,Pvec,A,B,Q,R,n)
P = reshape(Pvec,n,n);
Pdot = (P * A + A' * P - P * B * inv(R) * B' * P + Q);
dPdtvec = Pdot(:);
end


Instead of solving the discrete time Riccati Equation (which I have already implemented), I am trying to do everything in continuous time to get the solution. Thus, as you can see at the bottom, I have a function that computes the matrix Riccati ODE, and this gets fed into MATLAB's od45 solver, with the usual initial condition $$P(T) = Q_N$$, where $$Q_N$$ is the terminal cost matrix. This gives me $$P(t)$$, which checks out well. The problem is that when I go and simulate the dynamics, they diverge for some reason. I am not sure why.

Interestingly enough, if I use the the constant, steady-state gain $$K_{\infty} = R^{-1}B^\intercal P(0)$$, then everything works out nicely, but if I use time-varying gains (which are more accurate!), the solution doesn't work out. I think it may be due to some kind of discretization errors when simulating the dynamics, but I really don't know. Feel free to copy and past the script; any thoughts would be appreciated!

• How large is the largest deltat? – fibonatic Feb 9 '20 at 0:55
• Usually like 0.05 or 0.1 – Josh Pilipovsky Feb 9 '20 at 5:08
• I think I might have figured it out. The problem is that sometimes the horizon isn’t long enough so the solution stays transient. However, I’m not sure what the cut off would be, it would be an interesting question to consider – Josh Pilipovsky Feb 9 '20 at 5:09