xSol(t) = 
Icare doesnt have the answer
Ältere Kommentare anzeigen
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;
7 Kommentare
Walter Roberson
am 16 Apr. 2025
? You are not calling icare() ?
jowii
am 16 Apr. 2025
Walter Roberson
am 16 Apr. 2025
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
am 17 Apr. 2025
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.
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.
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.
. 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.
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))
sys = ss(A, B, C, D)
%% Remove the independent state x4
sys = minreal(sys)
% 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)))
@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)
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.
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.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)
% 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)
Antworten (1)
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)
The controllability matrix has one row that's all zeros
Co = ctrb(A,B)
Which means that the controllability is not full rank.
rank(Co)
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).
% Solusi Riccati
try
[K, P] = lqr(A, B, Q, R);
catch ME
ME.message
end
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))]
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);
Kategorien
Mehr zu State-Space Control Design finden Sie in Hilfe-Center und File Exchange
am 17 Apr. 2025
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
