how to simulate mpc controller?

disp('K matrisi:');
disp(K);
disp('B matrisi:');
disp(B);
disp('A matrisi:');
disp(A);

[Torsten]

And what is your specific question ?

[Sam Chak]

Hi @Mikail

When your post was first created, the title was something like "How to simulate LQR controller with ode45?" I am uncertain, but why did you edit the title to "How to simulate MPC controller?"

Answer 1

[Sam Chak]

Hi @Mikail,

I did not 'beautify' the plot. But the following outlines how you can simulate the LQR-controlled system using the ode45 solver. For simplicity, I used a linear system. While it is also possible to simulate the original nonlinear system, you will need to specify the 12 ODEs individually.

clear all;
clc;
close all;
%---------------------------------Star_X-----------------------------------
%----------------------------- Matematiksel--------------------------------
%-------------------------------modelleme---------------------------------- 
%--------------------------------------------------------------------------
% **Sayısal değerler**----------------------------------------------------
m_val = 25.0;    
Ix_val = 0.25;   
Iy_val = 5.28;   
Iz_val = 5.28;
theta_val=0;
phi_val=0;
psi_val=0;
g_val = 9.81;
wn_close = 400/2.5;
wn_open = 400/2.5;
dr = 1;
kp=70.779169670332;
ki=40.999999999919;
kd=40.3888999977265;
c=29.33685249238;
d=0;
%--------------------------------------------------------------------------
% **Sembolik değişkenleri tanımla**----------------------------------------
syms u v w p q r phi theta psi Fx Fy Fz L M N t1 t2 t3 h real
syms m Ix Iy Iz g real  
syms x1 y1 z1 real
%--------------------------------------------------------------------------
% **Durum değişkenleri**
x = [u; v; w; p; q; r; phi; theta; psi; x1; y1; z1];
% **Girdi vektörü**
u_vec = [Fx; Fy; Fz; L; M; N; t1; t2; t3];
f = [
    -g*cos(theta)*cos(psi)+Fx/(m-(0.5*t1 + 0.5*t2 + 0.5*t3))-0.016*u^2-q*w+r*v;  %-0.016*(u^2 + v^2 + w^2) - q*w + r*v// (-g*cos(theta)*cos(psi))
    -g*(sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi))+(Fy/(m-(0.5*t1 + 0.5*t2 + 0.5*t3)))-0.16*v^2-r*u+ p*w;
    -g*(cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi))+(Fz/(m-(0.5*t1 + 0.5*t2 + 0.5*t3)))-0.16*w^2-p*v + q*u;
    (L - q*r*(Iz - Iy))/Ix; %- cp*p
    (0.16*q^2-M)/Iy; % - cq*q
    (0.16*r^2-N)/Iz; % - cr*r
    p + (q*sin(phi) + r*cos(phi)) * tan(theta);
    q*cos(phi) - r*sin(phi);
    (q*sin(phi) + r*cos(phi)) / cos(theta);
    u*cos(theta)*cos(psi)+u*cos(theta)*sin(psi)-u*sin(theta);
    v*(cos(phi)*cos(psi)-cos(phi)*sin(psi)-sin(psi));
    w*(-cos(phi)*cos(theta)+cos(phi)*sin(theta)+sin(phi))
    ];
% **Jacobian matrislerini hesapla**    
A_matrix = jacobian(f, x);
B_matrix = jacobian(f, u_vec);
% **Denge noktaları**
x_equilibrium = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
u_equilibrium = [0; 0; 0; 0; 0; 0; 0; 0; 0];
% **A ve B matrislerini hesapla**
A_eq = subs(A_matrix, x, x_equilibrium);
A_eq = subs(A_eq, u_vec, u_equilibrium);
A_eq = subs(A_eq, [m, Ix, Iy, Iz, g,phi,theta,psi],[m_val, Ix_val, Iy_val, Iz_val, g_val,phi_val,theta_val,psi_val]);
A = double(A_eq); % Artık tüm değişkenler sayısal, double() çağrılabilir
B_eq = subs(B_matrix, [x; u_vec], [x_equilibrium; u_equilibrium]);
B_eq = subs(B_eq, [m, Ix, Iy, Iz, g,phi,theta,psi],[m_val, Ix_val, Iy_val, Iz_val, g_val,phi_val,theta_val,psi_val]);
B = double(B_eq); % Aynı şekilde B için de double() çağrılabilir
% **Kontrol edilebilirlik matrisi**
controllability = ctrb(A, B);
rank_controllability = rank(controllability);
% **LQR ile Optimal Kontrol Kazancı K Matrisi**
Q = diag([280,14,14,1,6,6,1,1,1,50,115,115]);
R = diag([0.052,0.06,0.06,2,2,2,1,1,1]);
K = lqr(A,B,Q,R);
% **Çıkış ve Direkt Geçiş Matrisleri**
C = eye(12);
D = zeros(12,9);
% **Başlangıç koşulları**
x_init = [0; 0; 0; 0; 0.7; 0.5; 0; 0; 0; 9; 0; 0];
t = 0:0.01:20;

%% ---------------------------------------------------
%% How to simulate the LQR-controlled system using the ode45 solver
%% ---------------------------------------------------

%% system
function dx = ode(t, x, A, B, K)
    u   = - K*x;
    dx  =   A*x + B*u;
end

%% call ode45
[t, x]  = ode45(@(t, x) ode(t, x, A, B, K), t, x_init);
plot(t, x), grid on, xlabel('t')
legend('x_1', 'x_2', 'x_3', 'x_4', 'x_5', 'x_6', 'x_7', 'x_8', 'x_9', 'x_{10}', 'x_{11}', 'x_{12}')

●

[Sam Chak]

Hi @Mikail

Since you used my suggested solution in your new post, would you consider clicking 'Accept' ✔ on the Answer to close this topic related to ode45? This will encourage PWM experts to focus on your new question.

[Sam Chak]

@Mikail Any update?