LQR Implementation in MATLAB

Question

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 paste the script; any thoughts would be appreciated!

Answer 1

You are dealing with multiple sources of errors. Namely, the ODE solver ode45 does not give an exact solution for $P(t)$ but a numerical approximation for it and the method you use to simulate the state is forward Euler, which is only a first order method. Since your system is unstable any perturbation might get amplified a lot. Also keep in mind the cost function that you are trying to minimize, because it might not be cost effective for finite horizon LQR to drive the state close to zero.

Fortunately one can improve the accuracy of many of the numerical results. Namely, there is an analytical solution for $P(t)$, and $x(t)$ can also be solved with ode45. The analytical solution for $P(t)$ can be found by using $P(t) = \bar{P} + V^{-1}(t)$, with $\bar{P}$ the stationary solution of $P(t)$ so by solving the related CARE. The dynamics of $V(t)$ can shown to be

$$ \dot{V} = V\,\mathcal{A}^\top + \mathcal{A}\,V - B\,R^{-1} B^\top, $$

with $\mathcal{A} = A - B\,R^{-1} B^\top \bar{P}$. By using vectorization and the Kronecker product that dynamics can also be written as

$$ \dot{z} = M\,z, $$

with $M = I \otimes \mathcal{A} + \mathcal{A} \otimes I$ and $z(t) = \text{vec}(V(t)) - M^{-1}\,\text{vec}(B\,R^{-1} B^\top)$. The solution for $z$ is given by

$$ z(t) = e^{M\,(t - \tau)}\,z(\tau). $$

So $P(t)$, given $P(T)$, can be obtained using

\begin{align} V(T) &= \left(P(T) - \bar{P}\right)^{-1}, \\ z(T) &= \text{vec}(V(T)) - M^{-1}\,\text{vec}(B\,R^{-1} B^\top), \\ V(t) &= \text{vec}^{-1}\!\left(e^{M\,(t - T)}\,z(T) + M^{-1}\,\text{vec}(B\,R^{-1} B^\top)\right), \\ P(t) &= \bar{P} + V^{-1}(t). \end{align}

I also did some simulations using your system/script while changing only your numerical solution for $P(t)$ with the analytical solution, the integration method for the dynamics of the state from forward Euler to ode45 or both. From those results it does seem that using the analytical solution $P(t)$ seemed to have to biggest impact.