Manually compute LQI() gains using Algebraic Riccati Equation / Hamiltonian

I'm attempting to compile a Matlab program for embedded use, and that program also includes lqr() and lqi(). Since neither are compilable functions for compiling, I'm solving LQR and LQI manually, ultimately hoping to end up only with functions that can be compiled.

I'm doing this via two Hamiltonian methods, with pole placement or to find the Algebraic Riccati Equation solution P. This is working correctly (solutions P and gains K match lqr() outputs) for a continuous-time basic plant without integrator. But, for the discrete case it's failing for both methods I try, with results that don't match Matlab's results (i won't go into the discrete implementation in detail, for clarity, unless asked)

My question is:

• Is there a better manual method to finding lqi gains, especially one that generalizes to the discrete case more easily? I'm having trouble using the below methods for the discrete case, even with needed changes (see links in this post) to handle the discrete case. I can create a separate post for troubleshooting that, but for now, i'm just curious if there are simpler manual methods -- or built-in methods that Matlab allows for compiling into executables.
• If I set Q = eye(3) for LQI case, it's suspicious that all lqi gains are 1's, and that lqr() and lqi(1:2) gains don't match. Why would gains be almost exactly 1 from Q = eye(3)?

Details

I'm using two manual processes. One uses poles placement via acker(), and the other works via Schur decomposition via schur(), as seen in this link.

Both methods work for LQR (plant without integral action). But neither works with LQI (plant with integral action), insofar as matching the base lqr() gains. Of course, the plant with integral action won't use the integral action in open-loop, so it's more a structural addition.

Method 1: Pole placement method

Method 2: Schur method

Code

clc
close all

A = [0 1; 
     -4e5 -400];
B = [0; 4e5];
C = [1 0];

sys = ss(A, B, C, 0);

% i denotes "integral"
Ai = [A, zeros(2,1); -C, 0];
Bi = [B; 0];
Ci = [C 0];
sys_aug = ss(Ai, Bi, Ci, 0);

% ========================
% LQR
% x = [x1; x2]
Q = eye(size(A));
Q(1,1) = 1e5;
Q(2,2) = 0.1;
R = 1;
K_lqr = lqr(sys, Q, R)

K_lqr = 1×2

315.2293 0.3177

% ~~~ Method 1 ~~~ 
hamiltonian = [A, -B/R*B';
               -Q, -A'];
eigs_H = eig(hamiltonian);
whichNegEig = find(real(eigs_H)<0);
% This matches K_lqr
K_hamilt = acker(A, B, eigs_H(whichNegEig))

K_hamilt = 1×2

315.2293 0.3177

% ~~~ Method 2 ~~~
[U, S] = schur(hamiltonian);
[U, S] = ordschur(U, S, 'lhp');

% Find U21 and U11: U is a square matrix.
[m,n] = size(U);
U11 = U(1:(m/2), 1:(n/2));
U21 = U((m/2+1):m, 1:(n/2));

% Find solution P to A.R.E.
P = U21 * inv(U11);
% Double-check solution using built-in care()
P_care = care(A, B, Q, R) ;
% This matches K_lqr
K_schur = inv(R) * B' * P

K_schur = 1×2

315.2294 0.3177

Acl_lqr_manual = [sys.A - sys.B * K_schur];
syscl_lqr_manual = ss(Acl_lqr_manual, B* K_lqr * [1;0], C, 0);

figure;
step(syscl_lqr_manual)
hold on;

% ========================
% LQI: Integral action
% x = [x1; x2; xi]

Qi = eye(size(Ai));
Qi(1,1) = Q(1,1);
Qi(2,2) = Q(2,2);
Qi(3,3) = 1e12;
Ri = 1;
K_lqr_i = lqr(sys_aug, Qi, Ri)

K_lqr_i = 1×3

1.0e+06 * 0.0009 0.0000 -1.0000

K_lqi_i2 = lqi(sys, Qi, Ri) % just double-check

K_lqi_i2 = 1×3

1.0e+06 * 0.0009 0.0000 -1.0000

% ~~~ Method 1 ~~~ 
hamiltonian_i = [Ai, -Bi/Ri*Bi';
               -Qi, -Ai'];
eigs_Hi = eig(hamiltonian_i);
whichNegEig_i = find(real(eigs_Hi)<0);
% This is all NaN due to 0 column
K_hamilt_i = acker(Ai, Bi, eigs_Hi(whichNegEig_i))

K_hamilt_i = 1×3

1.0e+06 * 0.0009 0.0000 -1.0000

% ~~~ Method 2 ~~~
[Ui, Si] = schur(hamiltonian_i);
[Ui, Si] = ordschur(Ui, Si, 'lhp');

% Find U21 and U11: U is a square matrix.
[m,n] = size(Ui);
U11i = Ui(1:(m/2), 1:(n/2));
U21i = Ui((m/2+1):m, 1:(n/2));

% Find solution P to A.R.E.
Pi = U21i * inv(U11i);
% Double-check solution using built-in care()
Pi_care = care(Ai, Bi, Qi, Ri);
% This ~matches K_lqr_i, but not K_lqi_i.
K_schur_i = inv(Ri) * Bi' * Pi

K_schur_i = 1×3

1.0e+05 * 0.0086 0.0000 -10.0000

Acl_lqi_manual = [sys.A - sys.B * K_schur_i(1:2), -sys.B * K_schur_i(3);
                -sys.C,                         0];

% Ref is through integ K(Ki*xi - x), ie
% Ref feedthrough only to xi, ie K(r-x), vs above w/ ref through integ K(Ki*xi - x)
Bcl_lqi_manual = [0; 0; 1];
Ccl_lqi_manual = [sys.C 0];

syscl_lqi_manual = ss(Acl_lqi_manual, Bcl_lqi_manual, Ccl_lqi_manual, 0);
step(syscl_lqi_manual)
step(sys)
legend('manual lqr', ' manual lqi', 'OL')

Answer 1

[Paul]

Hi John,

I just copy/pasted the code from the question (not the attached .m file) with some semicolons to suppress output and it seems to run fine. The gains for the plant all match when compute with the different methods and the gains for the augmented plant all match when computed with the different methods.

Perhaps I don't understand the concern with results from the code. Are you expecting some sort of commonality between the values of K_lqr and K_lqr_i?

Also, consider using place instead of acker, at least if the closed loop poles don't have multiplicity greater than one.

format long e
A = [0 1; 
     -4e5 -400];
B = [0; 4e5];
C = [1 0];
sys = ss(A, B, C, 0);
% i denotes "integral"
Ai = [A, zeros(2,1); -C, 0];
Bi = [B; 0];
Ci = [C 0];
sys_aug = ss(Ai, Bi, Ci, 0);
%% ========================
% LQR
% x = [x1; x2]
Q = eye(size(A));
R = 1;
K_lqr = lqr(sys, Q, R)

K_lqr = 1×2

4.142135623731785e-01 9.990015355327287e-01

% ~~~ Method 1 ~~~ 
hamiltonian = [A, -B/R*B';
               -Q, -A'];
eigs_H = eig(hamiltonian);
whichNegEig = find(real(eigs_H)<0);
% This matches K_lqr
K_hamilt = acker(A, B, eigs_H(whichNegEig))

K_hamilt = 1×2

4.142135623726624e-01 9.990015355327272e-01

% ~~~ Method 2 ~~~
[U, S] = schur(hamiltonian);
[U, S] = ordschur(U, S, 'lhp');
% Find U21 and U11: U is a square matrix.
[m,n] = size(U);
U11 = U(1:(m/2), 1:(n/2));
U21 = U((m/2+1):m, 1:(n/2));
% Find solution P to A.R.E.
P = U21 * inv(U11);
% Double-check solution using built-in care()
P_care = care(A, B, Q, R); 
% This matches K_lqr
K_schur = inv(R) * B' * P

K_schur = 1×2

4.142134701004187e-01 9.990015355179229e-01

%% ========================
% LQI: Integral action
% x = [x1; x2; xi]
Qi = eye(size(Ai));
Ri = 1;
K_lqr_i = lqr(sys_aug, Qi, Ri)

K_lqr_i = 1×3

1.000001499998889e+00 9.990029999992345e-01 -9.999999999999543e-01

K_lqi_i = lqi(sys, Qi, Ri) % just double-check

K_lqi_i = 1×3

1.000001499998889e+00 9.990029999992345e-01 -9.999999999999543e-01

% ~~~ Method 1 ~~~ 
hamiltonian_i = [Ai, -Bi/Ri*Bi';
               -Qi, -Ai'];
eigs_Hi = eig(hamiltonian_i);
whichNegEig_i = find(real(eigs_Hi)<0);
% This is all NaN due to 0 column
K_hamilt_i = acker(Ai, Bi, eigs_Hi(whichNegEig_i))

K_hamilt_i = 1×3

1.000001499998805e+00 9.990029999992499e-01 -9.999999999997840e-01

% ~~~ Method 2 ~~~
[Ui, Si] = schur(hamiltonian_i);
[Ui, Si] = ordschur(Ui, Si, 'lhp');
% Find U21 and U11: U is a square matrix.
[m,n] = size(Ui);
U11i = Ui(1:(m/2), 1:(n/2));
U21i = Ui((m/2+1):m, 1:(n/2));
% Find solution P to A.R.E.
Pi = U21i * inv(U11i);
% Double-check solution using built-in care()
Pi_care = care(Ai, Bi, Qi, Ri); 
% This ~matches K_lqr_i, but not K_lqi_i.
K_schur_i = inv(Ri) * Bi' * Pi

K_schur_i = 1×3

1.000001539340834e+00 9.990029998923534e-01 -9.999998277307853e-01

[John]

Thanks @Paul. Good point about being able to use place() as well.

The main question was this one:

• Is there a better manual method to finding lqi gains, especially one that generalizes to the discrete case more easily?
• i'm curious if there are simpler manual methods than what i am trying -- or built-in methods that Matlab allows for compiling into executables.

What i've tried in my post doesn't seem to easily generalize to the discrete case, which is ultimately what i require. I was wondering if there was an easier Matlab method that can be compiled.

And secondarily this question:

• Why would gains be almost exactly 1 from Q = eye(3)?
• It seems suspicious that all 3 gains are 1, although this could (admittedly) just be by chance. But it wasn't clear that i could trust the results, given all 3 being ~1

Here's what i tried so far for discrete, which seems to not be working as expected; see the comments.

%% ========================
% Discrete ARE solver
% https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#cite_note-2
format long e
T = 1 / 50e3;
A = [0 1; 
     -4e5 -400];
B = [0; 4e5];
C = [1 0];
sys = ss(A, B, C, 0);
sys_d = c2d(sys, T);

Ad = sys_d.A;
Bd = sys_d.B;
Cd = sys_d.C;

Qd = eye(size(Ad));
Rd = 1;
% Now try manually solving discrete ARE
% Why is lqrd() output NOT matching dlqr() or lqr()? This also does not yield
% a stable step()
K1_lqr_d = lqrd(Ad, Bd, Qd, Rd, T) 

K1_lqr_d = 1×2

1.0e+00 * 1.137704182594730e+05 2.448794261003702e-01

K2_lqr_d = dlqr(Ad, Bd, Qd, Rd)

K2_lqr_d = 1×2

-8.252026324065146e-01 1.225936907505343e-01

K3_lqr_d = lqr(sys_d, Qd, Rd)

K3_lqr_d = 1×2

-8.252026324065146e-01 1.225936907505343e-01

% ~~~ Method 1 ~~~ 
hamiltonian_d = [Ad + Bd*inv(Rd)*Bd'*inv(Ad)'*Qd, -Bd/Rd*Bd'*inv(Ad)';
               -inv(Ad)'*Qd, inv(Ad)'];
testEig_d = abs(eig(hamiltonian_d));
% For discrete, choose poles inside unit circle
whichEigLess1 = find(abs(testEig_d)<1);
% This DOES match SOME matlab results above
K_hamilt_d = acker(Ad, Bd, testEig_d(whichEigLess1))

K_hamilt_d = 1×2

-8.252026220741435e-01 1.225936907506411e-01

% ~~~ Method 2 ~~~ 
[U_d, S_d] = schur(hamiltonian_d);
[U_d, S_d] = ordschur(U_d, S_d, 'lhp');

% find U21 and U11 because U is a square matrix.
[m,n] = size(U_d);
U11_d = U_d(1:(m/2), 1:(n/2));
U21_d = U_d((m/2+1):m, 1:(n/2));

% Find the solution P to Algebraic Riccati Equation.
Pd = U21_d * inv(U11_d)

Pd = 2×2

1.0e+00 * -7.076022316861010e+04 6.048141384343934e-01 6.048141383899690e-01 -1.551946144815036e-02

Pd_care = care(Ad, Bd, Qd, Rd) % double-check -- does NOT match

Pd_care = 2×2

1.0e+00 * 6.420796481411136e+09 -5.007234915903532e+04 -5.007234915903532e+04 5.325836530340169e-01

% https://www.mathworks.com/help/control/ref/lti.lqr.html
% https://www.mathworks.com/help/control/ref/dlqr.html
% This does NOT match any matlab result above
K_schur_d = inv(Bd' * Pd * Bd + Rd) * (Bd'* Pd * Ad)

K_schur_d = 1×2

1.053373742291032e+01 -8.156570148301240e+00

[John]

Ah -- here was my mistake. I'm still interested if there is another portable method, that's easier than these two that i've tried.

The mistake was that this:

[Ud, Sd] = ordschur(Ud, Sd, 'lhp')
Pd_care = care(Ad, Bd, Qd, Rd)

needs to be the following, for the discrete case:

[Ud, Sd] = ordschur(Ud, Sd, 'udi')
Pd_dare = dare(Ad, Bd, Qd, Rd)

This does give the correct gains.
(I'm unable to close the code box and type past it, so hence having text in the codebox.
It'd be great to know how to get around this, as it's not the first time the editor seems stuck)

[Paul]

The first two inputs to lqrd should be the continuous time A and B matrices, not the discretized Ad, Bd matrices. The second two iputs to lqrd are the the Q and R matrices for the continuous time cost function. So, if you want the gains from lqr and dlqr to match lqrd, you'll also need to compute Qd and Rd in accordance with the equations shown in the Algorithms section of the lqrd doc page.

[John]

Thanks @Paul. What's the advantage to creating discrete-time Q, R, matrices? Per the documentation, "This command is useful to design a gain matrix for digital implementation after a satisfactory continuous state-feedback gain has been designed."

In that case, couldn't dlqr() be used? They can also return gains with a discrete matrices, but with continuous-time Q, R, matrices (I thought)

[Paul]

I've read this comment a few times and I don't fully understand the questions. I'll try to answer anyway basedon what I think the questions are poking at.

Suppose we have a continuous plant, modeled with Ac, Bc and we want to design an LQR controller that will be implemented in a discrete-time compensator.

Option 1: Design Kc_lqr using lqr based on Qc and Rc, where Qc and Rc define the continuous-time quadaratic cost integral. Usually this will involve some iteration on Qc and Rc until a satisfactory design is achieved. Because Kc_lqr is just a gain matrix, implement it direactly as a gain matrix in the discrete-time compensator implementation (just like one would do with any classical proportional control). Note that there is no guarantee that Kc_lqr is "optimal" in the mathematical sense when combined with the discrete-time approximation of the plant.

Option 2. Discretize the plant model using c2d (typically with zoh option) to get Ad and Bd. Design Kd_lqr using lqr or dlqr based on Qd and Rd, where Qd and Rd define the discrete-time quadratic cost sum. Usually this will involve some iteration on Qd and Rd until a satisfactory design is achieved. Implement Kd_lqr directly in discrete-time compensation. Kd_lqr is "optimal" for the discrete time approximation of the plant relative to the quadratic cost sum.

Option 3: Start with option 1, design K based on Ac, Bc, Qc and Rc. When the continuous-time design is satisfactory compute the discretized plant model Ad,Bd from Ac,Bc. Compute Qd,Rd from Qc,Rc so that the discrete-time quadratic cost sum in some sense approximates the continuous-time quadratic cost integral. This transformation from Qc,Rc to Qd,Rd can be done approximately or "exactly" (quotation marks because there is never anything exact going from continous to discrete); find a good text book or course notes to see how (or look at the Algorithms section of lqrd. Design Kd_lqr based on Ad, Bd, Qd, Rd using lqr or dlqr. Implement Kd_lqr in the discrete-time compensation. The idea here is to compute Kd_lqr that minimizes the discrete-time quadratic cost (computed from the discrete-time plant approximation), which in turn is close to "equivalent" in some sense as the continuous-time quadratic cost.

Option 4: Start with option 1, design K using lqr based on Ac, Bc, Qc and Rc. When the the continuous-time design is satisfactory compute Kd_lqrd using lqrd based on Ac, Bc, Qc and Rc. This option is doing the heavy lifting to compute the Qd, Rd (and Nd) that would be the ideal approach in Option 3.

Option 5: I suppose it's feasible to try iterate on Qc and Rc as inputs to lqrd to compute a Kd_lqrd until satisfactory design is achieved. I don't see any advantages to this approach.