Icare doesnt have the answer

Hi everybody !!

This is my code, but my P from icare doesnt have valuess

clc;
clear;

% Input Parameter
a_11    =   -0.0857343575298716;
a_12    =   -4.83449522782224;
a_21    =    0.000139892812156308;
a_22    =   -0.101382544341573;
b_1     = 2999.93494409188;
b_2     =  -49.6839141191581;
u0      =    1;
dt      =    1;
x       = [0; 
           0; 
           0; 
           0; 
           0];

% Matriks A (State matrix)
A       = [   1+a_11*dt,   a_12*dt,                                  0, 0, 0;
                a_21*dt, 1+a_22*dt,                                  0, 0, 0;
                      0,        dt,                                  1, 0, 0;
          -sin(x(3))*dt,         0, -u0*dt*sin(x(3))-cos(x(3))*dt*x(1), 1, 0;
           cos(x(3))*dt,         0,  u0*dt*cos(x(3))-sin(x(3))*dt*x(1), 0, 1];

% Matriks B (Input matrix)
B       = [b_1   0;
             0 b_2;
             0   0;
             0   0;
             0   0];

% Output matrix
C       = eye(5);               % Semua state terukur (untuk kemudahan observasi)

% Direct transmission matrix
D       = zeros(5, 2);

dt      = 1;                    % Time step (s)
T       = 61;                   % Total time (s)
N       = T/dt;                 % Total steps
t       = 0:dt:T;
n       = size(A,1);            % State dimension
m       = size(B,2);            % Control input dimension

% Inisialisasi noise (optional)
Qw      = 0.01*eye(n);          % Proses noise covariance
Rv      = 0.01*eye(n);          % Measurement noise covariance
dt      = 1;                    % Time step (s)
T       = 61;                   % Total time (s)
N       = T/dt;                 % Total steps
t       = 0:dt:T;
n       = size(A,1);            % State dimension
m       = size(B,2);            % Control input dimension

% Inisialisasi noise (optional)
Qw      = 0.01*eye(n);          % Proses noise covariance
Rv      = 0.01*eye(n);          % Measurement noise covariance

% Referensi
data        = xlsread('data_dubins.xlsx');
data_est    = xlsread('data_estimasi.xlsx');
xref        = zeros(n, T);
xref(1,:)   = data_est(1,:);        % v
xref(2,:)   = abs(data_est(2,:));   % r
xref(3,:)   = data(5,:);            % sudut
xref(4,:)   = data(2,:);            % x
xref(5,:)   = data(2,:);            % y

% LQT
Q       = zeros(n,n);
Q(1 ,1) = 10^1;
Q(2 ,2) = 10^1;
Q(3 ,3) = 10^1;
Q(4 ,4) = 10^1;
Q(5 ,5) = 10^1;                 % Penalti posisi dan kecepatan
R       = 0.1*eye(m);           % Penalti kontrol

% Solusi Riccati
[K, P]  = lqr(A, B, Q, R);      % Gain LQT

% Kalman
[L,~,~] = lqe(A, Qw, C, Qw, Rv);    % L adalah gain observer (Kalman)

% LQGt
x       = zeros(n,1);           % State sebenarnya
xhat    = zeros(n,1);           % Estimasi Kalman
y       = zeros(n,1);           % Output
X_log   = zeros(n,length(t));
Xhat_log= zeros(n,length(t));
U_log   = zeros(m,length(t));

for k = 1:length(t)
    % Tracking error
    xr = xref(:,k);
    
    % Hitung input referensi u_r (tracking feedforward term)
    ur = B\(A*xr - diff([xr, xr], 1, 2)/dt);  % Approximate dxr/dt
    
    % Sinyal kontrol LQG Tracking (Control signal)
    u = - K*(xhat - xr) + ur;
    
    % Simulasikan sistem nyata (real-time system)
    w = mvnrnd(zeros(n,1), Qw)';  % Process noise
    v = mvnrnd(zeros(n,1), Rv)';  % Measurement noise
    x = x + dt*(A*x + B*u + w);   % Integrasi Euler
    y = C*x + v;                  % Output dengan noise
    
    % Estimasi Kalman
    xhat = xhat + dt*(A*xhat + B*u + L*(y - C*xhat));
    
    % Logging
    X_log(:,k)      = x;
    Xhat_log(:,k)   = xhat;
    U_log(:,k)      = u;
end

% Plot
figure;
plot(X_log(1,:), X_log(3,:), 'b', 'LineWidth', 2); hold on;
plot(xref(1,:), xref(3,:), 'r--', 'LineWidth', 2);
xlabel('x (m)'); ylabel('y (m)');
title('LQG Tracking of 2D Path'); 
legend('Actual Path','Reference Path');
grid on;

[Walter Roberson]

? You are not calling icare() ?

[jowii]

Thank you robert, i was wrong to posting my code. Recently, i tried LQR and icare to get the values of K. But it seems the two of them have same problem. The problem with my icare is the values of matrix from icare in size (0,0). And with my LQR code i have this problem :

Error using lqr

Cannot compute a stabilizing LQR gain (the Riccati solution S and gain matrix K are infinite).

This could be because:

* A has unstable modes that are not controllable through B,

* Q,R,N values are too large,

* [Q N;N' R] is indefinite,

* The E matrix in the state equation is singular.

Error in LQGt (line 79)

[K,~,~] = lqr(A, B, Q, R);

>>

Thabk you Robert for reminding me, have a good day

[Walter Roberson]

I think we will need your two .xlsx files to test with.

lqr() is outside of my experience, so I probably will not be able to determine the causes myself.

[Sam Chak]

Hi Jowli

The simulation has several significant methodological issues that need to be addressed.

You attempt to manually simulate a continuous-time unstable nonlinear system using the Euler method with a for-loop approach and a relatively large time step of dt = 1. Consider using the ode45 solver.

You discretize the nonlinear dynamics to create a time-varying state matrix A(t) and apply the LQR algorithm to compute the feedback control gain matrix at each time step, hoping to stabilize the nonlinear system.

Due to the periodic nature of the sine and cosine terms, the state matrix becomes less than full rank at certain points in time, rendering the system uncontrollable. This causes the LQR to generate an error and terminate the simulation.

Before solving the fundamental control problem of stabilizing the nominal system, you added several components to the system, such as process noise, measurement noise, a state observer via a Kalman filter, and an LQG tracking controller. This complicates troubleshooting.

[Sam Chak]

Hi @jowii

Previously, I reviewed your post on a phone and overlooked some details. After a second look, it is clear that the line

x = x + dt*(A*x + B*u + w);   % Integrasi Euler

indicates the state matrix is not updated at every time step, implying that the matrix is a Jacobian linearized at the point x = [0; 0; 0; 0; 0]. However, I cannot ascertain whether the Jacobian matrix is correctly computed.

Upon closer examination of the original state-space model, you will see that ​. This represents one linearly independent eigenvector, which is unstable. Consequently, the controllability matrix is less than full rank. We do not know what went wrong; it is possible that the Jacobianization has gone wrong! Thus, if you isolate this state, you can get the LQR algorithm to work.

One final point concerns the line:

ur = B\(A*xr - diff([xr, xr], 1, 2)/dt);

The input matrix is not a square matrix. What is your intention with the backslash operation B\…? Additionally, the differentiation operation using diff() may not yield the desired result, as the time step dt = 1 is quite large from both Newton's and Euler's perspectives.

% Input Parameter
a_11    =   -0.0857343575298716;
a_12    =   -4.83449522782224;
a_21    =    0.000139892812156308;
a_22    =   -0.101382544341573;
b_1     = 2999.93494409188;
b_2     =  -49.6839141191581;
u0      =    1;
dt      =    1;
x       = [0; 
           0; 
           0; 
           0; 
           0];

% Matriks A (State matrix)
A       = [   1+a_11*dt,   a_12*dt,                                  0, 0, 0;
                a_21*dt, 1+a_22*dt,                                  0, 0, 0;
                      0,        dt,                                  1, 0, 0;
          -sin(x(3))*dt,         0, -u0*dt*sin(x(3))-cos(x(3))*dt*x(1), 1, 0;
           cos(x(3))*dt,         0,  u0*dt*cos(x(3))-sin(x(3))*dt*x(1), 0, 1];

% Matriks B (Input matrix)
B       = [b_1   0;
             0 b_2;
             0   0;
             0   0;
             0   0];

% Output matrix
C       = eye(5);               % Semua state terukur (untuk kemudahan observasi)

% Direct transmission matrix
D       = zeros(5, 2);

% check rank of controllability matrix of original system
rk  = rank(ctrb(A, B))

rk = 4

sys = ss(A, B, C, D)

sys =
 
  A = 
              x1         x2         x3         x4         x5
   x1     0.9143     -4.834          0          0          0
   x2  0.0001399     0.8986          0          0          0
   x3          0          1          1          0          0
   x4          0          0          0          1          0
   x5          1          0          1          0          1
 
  B = 
           u1      u2
   x1    3000       0
   x2       0  -49.68
   x3       0       0
   x4       0       0
   x5       0       0
 
  C = 
       x1  x2  x3  x4  x5
   y1   1   0   0   0   0
   y2   0   1   0   0   0
   y3   0   0   1   0   0
   y4   0   0   0   1   0
   y5   0   0   0   0   1
 
  D = 
       u1  u2
   y1   0   0
   y2   0   0
   y3   0   0
   y4   0   0
   y5   0   0
 
Continuous-time state-space model.

%% Remove the independent state x4
sys = minreal(sys)

1 state removed.

sys =
 
  A = 
              x1         x2         x3         x4
   x1     0.9143     -4.834          0          0
   x2  0.0001399     0.8986          0          0
   x3          0          1          1          0
   x4          1          0          1          1
 
  B = 
           u1      u2
   x1    3000       0
   x2       0  -49.68
   x3       0       0
   x4       0       0
 
  C = 
       x1  x2  x3  x4
   y1   1   0   0   0
   y2   0   1   0   0
   y3   0   0   1   0
   y4   0   0   0   0
   y5   0   0   0   1
 
  D = 
       u1  u2
   y1   0   0
   y2   0   0
   y3   0   0
   y4   0   0
   y5   0   0
 
Continuous-time state-space model.

% check the rank again
rk  = rank(ctrb(sys.A, sys.B))

rk = 4

% Now, LQR should work
K   = lqr(sys, eye(sqrt(numel(sys.A))), eye(size(sys.B, 2)))

K = 2×4

1.0010 0.0133 0.7951 2.2077 -0.0002 -1.0725 -2.7726 -0.7372

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

[Sam Chak]

@jowii, Perhaps you'll get this faster.

%% Solving in MATLAB
syms x(t)
eqn     = diff(x, t) == 1*x;    % dx/dt = 1*x
xSol(t) = dsolve(eqn)

xSol(t) = 

If the constant ​ is non-zero, there are two types of exponential functions: one that grows exponentially and another that decays exponentially. The solution clearly indicates that it is of the growing type. However, if the constant ​ is zero, the solution remains at zero indefinitely.

[Sam Chak]

Hi @jowii

I do not have experience with running icare(). Could you demonstrate how you compute it? Initially, I thought you intended to use icare() to design the State-Dependent Riccati Equation (SDRE) controller for the nonlinear system.

% Input Parameter
a_11    =   -0.0857343575298716;
a_12    =   -4.83449522782224;
a_21    =    0.000139892812156308;
a_22    =   -0.101382544341573;
b_1     = 2999.93494409188;
b_2     =  -49.6839141191581;
u0      =    1;
dt      =    1;
x       = [0; 
           0; 
           0; 
           0; 
           0];

% Matriks A (State matrix)
A       = [   1+a_11*dt,   a_12*dt,                                  0, 0, 0;
                a_21*dt, 1+a_22*dt,                                  0, 0, 0;
                      0,        dt,                                  1, 0, 0;
          -sin(x(3))*dt,         0, -u0*dt*sin(x(3))-cos(x(3))*dt*x(1), 1, 0;
           cos(x(3))*dt,         0,  u0*dt*cos(x(3))-sin(x(3))*dt*x(1), 0, 1];

% Matriks B (Input matrix)
B       = [b_1   0;
             0 b_2;
             0   0;
             0   0;
             0   0];

% Output matrix
C       = eye(5);               % Semua state terukur (untuk kemudahan observasi)

% Direct transmission matrix
D       = zeros(5, 2);

% check rank of controllability matrix of original system
rk  = rank(ctrb(A, B));
sys = ss(A, B, C, D);

%% Remove the independent state x4
sys = minreal(sys)

1 state removed.

sys =
 
  A = 
              x1         x2         x3         x4
   x1     0.9143     -4.834          0          0
   x2  0.0001399     0.8986          0          0
   x3          0          1          1          0
   x4          1          0          1          1
 
  B = 
           u1      u2
   x1    3000       0
   x2       0  -49.68
   x3       0       0
   x4       0       0
 
  C = 
       x1  x2  x3  x4
   y1   1   0   0   0
   y2   0   1   0   0
   y3   0   0   1   0
   y4   0   0   0   0
   y5   0   0   0   1
 
  D = 
       u1  u2
   y1   0   0
   y2   0   0
   y3   0   0
   y4   0   0
   y5   0   0
 
Continuous-time state-space model.

% check the rank again
rk  = rank(ctrb(sys.A, sys.B));

% Now, LQR should work
K   = lqr(sys, eye(sqrt(numel(sys.A))), eye(size(sys.B, 2)));

%% Playing with icare
n   = size(sys.A, 1);
m   = size(sys.B, 2);
p   = size(sys.C, 1);

BB  = [sys.B, zeros(n,p)];
Q   =  sys.C'*sys.C;
R   =  blkdiag(eye(m), -eye(p));
G   = -sys.B*sys.B';
[P, K, L] = icare(sys.A, BB, Q, R, [], [], G)

P = 4×4

0.0002 0.0000 0.0002 0.0005 0.0000 0.0150 0.0392 0.0106 0.0002 0.0392 2.8203 0.7835 0.0005 0.0106 0.7835 2.2052

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

K = 7×4

0.7076 0.0067 0.5539 1.5588 -0.0001 -0.7434 -1.9486 -0.5248 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

L = 
   1.0e+03 *

  -0.0015 + 0.0003i
  -0.0015 - 0.0003i
  -0.0703 + 0.0000i
  -4.2425 + 0.0000i

Answer 1

[Paul]

Hi jowii,

% Input Parameter
a_11 = -0.0857343575298716;
a_12 = -4.83449522782224;
a_21 = 0.000139892812156308;
a_22 = -0.101382544341573;
b_1 = 2999.93494409188;
b_2 = -49.6839141191581;
u0 = 1;
dt =1;
x = [0; 0; 0; 0; 0];
% Matriks A
A = [  1+a_11*dt ,                              a_12*dt,            0,                         0, 0;
    a_21*dt ,                            1+a_22*dt,            0,                         0, 0;
    0,                                 dt,                1,                         0, 0;
    -sin(x(3))*dt,                                0, -u0*dt*sin(x(3))-cos(x(3))*dt*x(1),       1, 0;
    cos(x(3))*dt,                                 0,  u0*dt*cos(x(3))-sin(x(3))*dt*x(1),       0, 1;
    ];
% Matriks B
B =[b_1 0;
    0 b_2;
    0 0;
    0 0;
    0 0];
%{
C = eye(5);   % Semua state terukur (untuk kemudahan observasi)
D = zeros(5, 2);
dt = 1;        % Time step (s)
T = 61;          % Total time (s)
N = T/dt;        % Total steps
t = 0:dt:T;
%}
n = size(A,1);   % State dimension
m = size(B,2);   % Control input dimension
%{
% Inisialisasi noise (optional)
Qw = 0.01 * eye(n);   % Proses noise covariance
Rv = 0.01 * eye(n);   % Measurement noise covariance
dt = 1;        % Time step (s)
T = 61;          % Total time (s)
N = T/dt;        % Total steps
t = 0:dt:T;
n = size(A,1);   % State dimension
m = size(B,2);   % Control input dimension
% Inisialisasi noise (optional)
Qw = 0.01 * eye(n);   % Proses noise covariance
Rv = 0.01 * eye(n);   % Measurement noise covariance
%}
% Referensi
%{
data = xlsread('data_dubins.xlsx');
data_est = xlsread('data_estimasi.xlsx');
xref = zeros(n, T);
xref(1,:) = data_est(1,:); %v
xref(2,:) = abs(data_est(2,:)); %r
xref(3,:) = data(5,:); %sudut
xref(4,:) = data(2,:); % x
xref(5,:) = data(2,:); %y
%}
% LQT
Q = zeros(n,n);
Q (1 ,1) = 10^1;
Q (2 ,2) = 10^1;
Q (3 ,3) = 10^1;
Q (4 ,4) = 10^1;
Q (5 ,5) = 10^1;    % Penalti posisi dan kecepatan
R = 0.1 * eye(m);           % Penalti kontrol

All of the eigenvalues of the A matrix represent untable modes

eig(A)

ans = 
   1.0000 + 0.0000i
   1.0000 + 0.0000i
   0.9064 + 0.0248i
   0.9064 - 0.0248i
   1.0000 + 0.0000i

The controllability matrix has one row that's all zeros

Co = ctrb(A,B)

Co = 5×10

1.0e+04 * 0.3000 0 0.2743 0.0240 0.2506 0.0435 0.2287 0.0592 0.2086 0.0715 0 -0.0050 0.0000 -0.0045 0.0001 -0.0040 0.0001 -0.0036 0.0001 -0.0032 0 0 0 -0.0050 0.0000 -0.0094 0.0001 -0.0134 0.0002 -0.0170 0 0 0 0 0 0 0 0 0 0 0 0 0.3000 0 0.5743 0.0191 0.8249 0.0532 1.0537 0.0989

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

Which means that the controllability is not full rank.

rank(Co)

ans = 4

Hence, the system is uncontrollable and the uncontrollable mode must be unstable because all of the modes are unstable (the uncontrollable mode must be the mode at s = 1, I believe).

Hence, LQR cannot obtain a stabilizing controller and lqr returns an error.

% Solusi Riccati
try
[K, P] = lqr(A, B, Q, R);
catch ME
    ME.message
end

ans = 
    'Cannot compute a stabilizing LQR gain (the Riccati solution S and gain matrix K are infinite). 
     This could be because: 
       * (A,E) has unstable modes that are not controllable through B, 
       * Q,R,N values are too large, 
       * [Q N;N' R] is indefinite, 
       * The E matrix in the state equation is singular.'

The relevant issue is probably the first listed above, but I don't know what "(A,E)" means in that context (that message looks very strange to me).

The doc page lqr says "The pair (A,B) must be stabilizable," which is not the case for your model and is why LQR can't work.

Looking again at how the A matrix is formed, did you mean for the plant to be modeled in discrete time with sampling period dt = 1 sec? If so, then we shouldn't be using that form of lqr, which is applicable for continuous time systems.

Assuming a discrete-time model, the A matrix has three modes on the unit circle.

format long e
[eig(A) abs(eig(A))]

ans = 
      1.000000000000000e+00 + 0.000000000000000e+00i      1.000000000000000e+00 + 0.000000000000000e+00i
      1.000000000000000e+00 + 0.000000000000000e+00i      1.000000000000000e+00 + 0.000000000000000e+00i
      9.064415490642777e-01 + 2.480110270034033e-02i      9.067807764643007e-01 + 0.000000000000000e+00i
      9.064415490642777e-01 - 2.480110270034033e-02i      9.067807764643007e-01 + 0.000000000000000e+00i
      1.000000000000000e+00 + 0.000000000000000e+00i      1.000000000000000e+00 + 0.000000000000000e+00i

I'm pretty sure one mode at z = 1 is not controllable from B, so the pair (A,B) is not stabilizable, which is why dlqr, which should be used for discrete-time systems, also fails.

[K,S,e] = dlqr(A,B,Q,R);

Error using dlqr (line 34)
Cannot compute a stabilizing LQR gain (the Riccati solution S and gain matrix K are infinite).
This could be because:
* (A,E) has unstable modes that are not controllable through B,
* Q,R,N values are too large,
* [Q N;N' R] is indefinite,
* The E matrix in the state equation is singular.