How to use pwm with lqr controller?

I want see Fx,Fy and Fz in discrete form using pwm. But ı have an lqr controller and Fx, Fy and Fz depending on u . Please help me with it.

clear all;
clc;
close all;

% **Sistem Parametreleri**
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;
rho_val = 1.225;
Sx_val = 0.00;% x eksenindeki tahmini alan
Sy_val = 0.0; % y eksenindeki tahmini alan
Sz_val = 0.0; % z eksenindeki tahmini alan
Cx_val = 0.0; % Aerodinamik katsayı
Cy_val = 0.0; % Aerodinamik katsayı
Cz_val = 0; % Aerodinamik katsayı
Cl_val = 0.0; Cm_val = 0.0; Cn_val = 0.0;
d=0.75;
kp=70.779169670332;
ki=40.999999999919;
kd=40.3888999977265;
c=29.33685249238;
wn_close = 400/2.5;
wn = 400/2.5;
dr = 1;
% **Sembolik Değişkenler**
syms u v w p q r phi theta psi Fx Fy Fz L M N t1 t2 t3 p_x p_y p_z  real
syms m Ix Iy Iz g real  
syms rho Sx Sy Sz Cx Cy Cz Cl Cn Cm real

% **Durum Değişkenleri ve Girdi Vektörü**
x = [u; v; w; p; q; r; phi; theta; psi; p_x; p_y; p_z];
u_vec = [Fx; Fy; Fz; L; M; N];

% **Dinamik Denklem**
f = [
    -g*cos(theta)*cos(psi)+Fx/m-0.5*rho*Sx*Cx*u^2/m-q*w+r*v;  
    -g*(sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi))+(Fy/m)-0.5*rho*Sy*Cy*v^2/m-r*u+ p*w;
    -g*(cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi))+(Fz/m)-0.5*rho*Sz*Cz*w^2/m-p*v + q*u;
    (0.5*rho*Sx*d*Cl*L^2-L)/Ix; 
    (0.5*rho*Sy*d*Cm*q^2-M)/Iy; 
    (0.5*rho*Sz*d*Cn*r^2-N)/Iz;
    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 Matrisleri**
A_matrix = jacobian(f, x);
B_matrix = jacobian(f, u_vec);

% **Denge Noktaları**
x_equilibrium = zeros(12,1);
u_equilibrium = zeros(6,1);

% **A ve B Matrislerinin Değeri**
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);

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);

% **Kontrol Edilebilirlik**
controllability = ctrb(A, B);
rank_controllability = rank(controllability);

% **LQR Kazanç 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]);
K = lqr(A, B, Q, R);

% **Başlangıç Koşulları ve Referans**
xref = [-0.5; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % Hedef durum
x_init = [0; 0; 0; 0; 0.7; 0.5; 0; 0; 0; 9; 0; 0]; % Başlangıç durumu

% **ODE Çözümü**
tspan = 0:0.01:20;
u_history = zeros(6, length(tspan));

% ODE Çözümü ve Kontrol Girdilerini Kaydetme
[t, x] = ode45(@(t, x) ode_with_control(t, x, A, B, K, xref), tspan, x_init);
for i = 1:length(t)
    [~, u_history(:, i)] = ode_with_control(t(i), x(i, :)', A, B, K, xref);
end

% **Sonuçları Çizdirme**
figure;
subplot(5,1,1);
plot(t, x(:,1:3), 'LineWidth', 1.5);
legend('x_1', 'x_2', 'x_3');
xlabel('Zaman (s)');
ylabel('Değerler');
title('Durum Değişkenleri 1-3');
grid on;

subplot(5,1,2);
plot(t, x(:,4:6), 'LineWidth', 1.5);
legend('x_4', 'x_5', 'x_6');
xlabel('Zaman (s)');
ylabel('Değerler');
title('Durum Değişkenleri 4-6');
grid on;

subplot(5,1,3);
plot(t, x(:,7:9), 'LineWidth', 1.5);
legend('x_7', 'x_8', 'x_9');
xlabel('Zaman (s)');
ylabel('Değerler');
title('Durum Değişkenleri 7-9');
grid on;

subplot(5,1,4);
plot(t, x(:,10:12), 'LineWidth', 1.5);
legend('x_{10}', 'x_{11}', 'x_{12}');
xlabel('Zaman (s)');
ylabel('Değerler');
title('Durum Değişkenleri 10-12');
grid on;

subplot(5,1,5);
plot(t, u_history(1,:), 'r', 'LineWidth', 1.5);
hold on;
plot(t, u_history(2,:), 'g', 'LineWidth', 1.5);
plot(t, u_history(3,:), 'b', 'LineWidth', 1.5);
legend('F_x', 'F_y', 'F_z');
xlabel('Zaman (s)');
ylabel('Kuvvet (N)');
title('Uygulanan Kuvvetler');
grid on;
hold off;

●

% **ODE Fonksiyonu**
function [dx, u] = ode_with_control(t, x, A, B, K, xref)
    u = -K * (x);  % LQR ile kontrol
    dx = A * x + B * u;   % Sistem dinamikleri
end

[Mathieu NOE]

hello

just to be sure , you mean pwm = pulse width modulation ? like in doc : PWM Generator ?

you need to create a carrier (triangular waveform) at a much higher frequency then your signal frequency content , and make a logical comparison between the signal and the carrier (as explained in the link)

Answer 1

[Mathieu NOE]

hello again

I am not sure what it brings here to convert your linear outputs to pwm , but a simple demo below if you want to try it and adapt it to your code :

%% demo pwm output
tf = 0.5; % final time
dt = 1e-2;% signal sampling rate
t = (0:dt:tf);
freq = 2; % signal frequency
signal = 0.95*sin(2*pi*freq*t); % !! signal amplitude must be normalized to +/- 1 range !!

% pwm signal 
factor = 100; % upsample factor (between incoming signal sampling rate and PWM carrier sampling rate
dtpwm = dt/factor; % much faster sampling rate
tpwm = (0:dtpwm:tf);
% carrier signal = triangular waveform
signal_carrier = sawtooth(2*pi*freq*factor*tpwm, 0.5); % 0.5 specifies a triangular waveform

%% compare signal to carrier
% first you have to resample the signal to the carrier sampling freq
signal2 = interp1(t,signal,tpwm);
% apply logical conditions to out_pwm
ind_pos = (signal2>signal_carrier);
ind_neg = (signal2<signal_carrier);
out_pwm = zeros(size(signal_carrier)); % initialize out_pwm to zero
out_pwm(ind_pos) = +1;
out_pwm(ind_neg) = -1;

% low pass filter the PWM output => should reproduce input waveform
[b,a] = butter(2,freq*dt/2);
out_pwm_filtered = filtfilt(b,a,out_pwm);

% plot
subplot(2,1,1),plot(t,signal,tpwm,signal_carrier);
legend('signal','pwm carrier')
xlabel('time(s)');
ylim([-1.2 1.2])
subplot(2,1,2),plot(tpwm,out_pwm,tpwm,out_pwm_filtered);
ylim([-1.2 1.2])
legend('pwm signal','lowpass filtered pwm signal')
xlabel('time(s)');

[Mathieu NOE]

hello again

problem solved ?

[Mikail]

Thanks for your reply . I consider again and decide that I won't use lqr controller and pwm because of active control limitation so ı didn't use your code but it will be very helpful on my future project.

[Sam Chak]

@Mikail, After making a decision, have you considered the alternatives?