Info

此问题已关闭。 请重新打开它进行编辑或回答。

Icare or P matrix doesnt have a values

1 次查看（过去 30 天）
jowii
jowii 2025-4-16
关闭： John D'Errico 2025-4-16
Hi everyone, i upload the wrong code before :))
This is my code, but my P from icare doesnt have the values
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
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
% Solusi Riccati
[P,~,~] = icare(A, B, Q, R);
K = R / (B'*P);% 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
u = -K * (xhat - xr) + ur;
% Simulasikan sistem nyata
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;
Incorrect dimensions for matrix multiplication. Check that the number of columns in the first matrix matches
the number of rows in the second matrix. To operate on each element of the matrix individually, use TIMES (.*)
for elementwise multiplication.
Error in LQGt (line 78)
K = R / (B'*P);% Gain LQT
Related documentation
>>
This erorr because matrix P is in the size (0x0)

此问题已关闭。

回答（0 个）

此问题已关闭。

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by