LQI controller is weirdly slow in response and doesn't always reach a goal on plant

I am trying to develop a servomechanism LQI controller. The servo is supposed to act as an actuator for a brake pedal - it is connected to a pedal via steel wire and therefore pushes on the pedal similarily to how a driver would. My goal was to first implement the control in MATLAB step by step and then rewrite it to embedded C. In simulation the controller behaves in a very satisfactory way, but when moving to plant, the response seems very delayed. Additionaly, in simulation I have neved had any issues with goal reaching, but on plant it turned out that the maximum the controller can achieve is 10 bars, and even when the goal pressure is higher, it never reaches it, just settles on the lower value. What is weird is that there is no issue in reaching lower pressures. I have tried to change the integral gain value since in the very beginning of testing, the maximum reachable pressure was even lower, and somehow changing the gain got me up to 10 bars - but never more. I am new to control theory. I need this system to be way faster and I want to make the delay the smallest possible, preferably around 30 ms.

Below I'm adding two pictures - one is from simulation and the other one directly from the plant. The delay on the plant is around 1 second (around 1100 ms).

% System identification
A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;
    0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
    -0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
    0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
    -0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];

B = [-0.0691336950664988;
    0.298054008136356;
    -0.101282668881553;
    -0.135571075515549;
    -0.197136001368224];

C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];

D = 0;

sys = ss(A, B, C, D, 0.02);
step(sys);

[A_size_row, A_size_column] = size(A);
Q = 70*eye(A_size_row+1);
R = 5;

A_aug = [A, zeros(A_size_column, 1); -C, 0];
B_aug = [B; 0];

[K, ~, ~] = lqr(A_aug, B_aug, Q, R); % LQR solver
K_x = K(1:5);
K_i = K(6);

dt = 0.02;  % time step;
T = 1000;  % sim time in milliseconds (or is it seconds?)
time = 0:dt:T;

x = [0; 
    0;
    0;
    0;
    0];  % initial state
x_i = 0;  % initial integrator state
y = [];  % output storage
u = [];  % control effort storage
ref = [];  % reference signal storage
states = []; % state storage
r=0;

% Simulation loop
for t = time
    % Reference signal
    if t == 50
        r = 5;
    end
    
    % Control law
    u_t = -K_x * x - K_i * x_i;

    % System dynamics
    x_dot = A * x + B * u_t;
    x = x + x_dot * dt;
    
    % Integrator dynamics
    y_t = C * x;
    x_i_dot = r - y_t;
    x_i = x_i + x_i_dot * dt;
    
    % Store results
    y = [y, y_t];
    u = [u, u_t];
    ref = [ref, r];
    states = [states, x]; % Store state vector
end

% Plot results
figure;
subplot(3,1,1);
plot(time, ref, 'r--', 'LineWidth', 1.5);
hold on;
plot(time, y, 'b', 'LineWidth', 1.5);
xlabel('Time (ms)');
ylabel('Output y');
title('System Output vs Reference Signal');
legend('Reference', 'Output');
grid on;

subplot(3,1,2);
plot(time, u, 'g', 'LineWidth', 1.5);
xlabel('Time (ms)');
ylabel('Control Effort u');
title('Control Effort');
grid on;

subplot(3,1,3);
plot(time, states, 'LineWidth', 1.5);
xlabel('Time (ms)');
ylabel('States');
title('States over Time');
legend('x1', 'x2', 'x3', 'x4', 'x5');
grid on;

[Mathieu NOE]

hello

maybe the simulation gives you good results because the LQI regulator is well designed vs the model you give him - this trigger my question , how sure are you about your model ? - looks like a first oder + delay , which BTW could probably be controlled with some simpler PID controller ?

also the 200 ms delay in your ss model looks quite large and again this makes me believe there is probably a issue between reality and model. How can your actuator have such dead time ??

second as your LQI control law is more difficult to implement (vs PID) in real time , maybe there is a bug somewhere ... did you compare the C code vs the matlab code before real time testing ?

[Wiktoria]

Hello!

Thank you for your answer. There might be some issue about the model - I used system identification toolbox and tried to follow MATLAB guides about how to do that. After I got the response to the step from the real time plant, I tried to fit it with the toolbox using the "estimate state-space models" option.

The actuator itself doesn't have such a big delay at all! Actually, the delay is around 20 ms only. When I set the pulse directly ("by hand") it's very fast, but when controller does that - it suddenly is very slow. So you're probably right, it might be the issue with the model. I compared the code from matlab before testing it on plant - it seemed identical.

My goal is to use LQI or LQR as I heard it's more precise and robust, as well as I want to simply try to implement it to gain knowledge (and it's also my project that I need to finish). Is there any way to make the response faster or to fit the proper, faster model? Thank you for your help!

[Paul]

Hi Wiktoria,

These lines define the system with a discrete-time model:

A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;
    0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
    -0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
    0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
    -0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];
B = [-0.0691336950664988;
    0.298054008136356;
    -0.101282668881553;
    -0.135571075515549;
    -0.197136001368224];
C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];
D = 0;
sys = ss(A, B, C, D, 0.02);
isdt(sys)

ans = logical
   1

But these lines

A_aug = [A, zeros(A_size_column, 1); -C, 0];
B_aug = [B; 0];

are what would be used to augment a continuous-time plant with an integrator.

And this form of the call to lqr

[K, ~, ~] = lqr(A_aug, B_aug, Q, R); % LQR solver

only applied for continuous-time systems. So it appears there is a disconnect between the form of the model and how the control law is being designed.

If you want to use lqr, you'll need to augment the plant with a discrete-time integrator and call lqr with an ss object as the first argument. Alternatively, you can use lqi to design the gains.

Finally, the simulation loop is treating the state space model as if it describes the system dynamics in continuous time, even though sys is a discrete-time model.

Before doing anything else the actual domain (continuous or discrete) of A,B,C,D needs to be established.

[Wiktoria]

Hello, thank you for your answer!

I have checked that the augmentation for discrete-time plant is supposedly

where m is a number of states. I don't quite understand if it is 5x5 matrix? But from what I understand, if A is 5x5 matrix, -C is 1x5, therefore zeros must be 5x1, so I matrix should be 1x1?

A_aug = [A, zeros(A_size_column, 1); -C, 1];
B_aug = [B; 0];

Aufter augmenting I computed the gains as you mentioned, using the formula below:

sys = ss(A, B, C, D, 0.02);
[K2,S,e] = lqi(sys,Q,R);

After generating a step with a closed loop system matrix, it turned out it looked pretty good.

Acl=A-B*K(1:5);
sys_lqr=ss(Acl,B,C,D, 0.02);
step(sys_lqr);

I am confused though, you mentioned that the simulation loop is treating the state space model as if it describes the system dynamics in continuous time, even though sys is a discrete-time model. Could you elaborate what you mean? Or is there perhaps any way to get to know how it should look like for discrete time? I tried to use the gains from lqi in the loop I have now (so the one for continuous time) and it is unstable (which isn't really surprising). How should the loop look like? I am new to control theory and didn't know there is a difference in how the loop is treating the space model - is there a difference in equations? How to correct it?

[Paul]

Let's go back to step 1. You have a model with A,B,C,D matrices defined as

A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;
    0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
    -0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
    0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
    -0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];
B = [-0.0691336950664988;
    0.298054008136356;
    -0.101282668881553;
    -0.135571075515549;
    -0.197136001368224];
C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];
D = 0;

Do these matrices represent a model of the form:

dx/dt = A*x(t) + B*u(t)

y(t) = C*x(t) + D*u(t)

or of the form:

x(k+1) = A*x(k) + B*u(k)

y(k) = C*x(k) + D*u(k)

?

If the former, what was the intent of the 0.02 parameter in the line

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

If the latter, is the 0.02 supposed to be the sampling period? Like maybe you developed the A,B,C,D matrices using system indentification on data that was sampled every 0.02 seconds?

[Wiktoria]

It was the latter, I wanted to represent a form

The 0.02s is supposed to be the sampling period, yes. I have developed the system using System Identification Toolbox using data that was sampled every 0.02s. In the toolbox, I was supposed to input the sampling period, so that's what I did.

[Paul]

Discussion continued at this answer.

[Pavl M.]

Attn.:

The original idea to employ the Hermittian matrix for the specific closed loop agent design and simulation:

Acl=A-B*K; % which is H matrix in my answer.

sys_lqr=ss(Acl,B,C,D, s_time);

step(sys_lqr);

is my, not of that Paul, please let's keep order who is who.

It is really good question.

Kindly see my recent the solution update:

(Updated, 17 Nov. 2024, see chk1.m script, Final for the numerical LQR, LQI gains design):

Here is a solution for much more faster LQ controller(agent) working in loop ( see my answer code + m going to add simulation of your loop with noise addition) with reduced oscillations by pre-filter I developed(constructed) novel (plagiarism free, not taken from any published manuscript, whatever indeed works in soft real time embedded). What is better for you LQR(no augmentation of matrices A,B,C,D, is required) or LQI(needs augmentation of A,B,C,D, matrices)Carefull selection of Q,R,N matrices are similar in goal vs the process of PID tuning.

Let's see maybe you can use the controller (smart agent for which the good K gains finding we have completed) with your system without complex Particle Filter or Kalman state X estimator if to rely on your linearized matrices A,B,C,D (please check are you sure about the matrices, and what is the actual physical system to control in non-linear, time varying case? whether they work in embedded code?) solely then the state X can be computed inside the embedded code from equation

X_dot = A*X+B*U,

then the control output can be computed as:

LQR:

U = (setpoint - y_from_real_phys_system)*Klqr(N+1) - (Klqr1.. KlqrN)*(X1 .. XN) (designed so far with augmented)

U = (setpoint - y_from_real_phys_system)*Klqr(1) - (Klqr2.. KlqrN)*(X2 .. XN)(not augmented)

or U = F1(setpoint - y_from_real_phys_system) - (Klqr1...KlqrN)*(X1..XN)

or (see chk1.m script uploaded)

LQI:

u = DiscreteIntegratorApproximator(r - y_from_real_phys_system)*Klqi(N+1) - Klqi(1..N)*X(1..N) (most precise)

or

u = DiscreteIntegratorApproximator(r - y_from_real_phys_system)*Klqi1 - Klqi(2..N+1)*X(1..N). (depending on augmentation from beginning or augmentation at end)

The control u computed on your embedded board relying on the actual real physical system estimation matrices A,B,C,D, hard coded in your embedded code and using actual physical system output y, the control u computed you just connect via appropriate transducer (signal conditioner) to your actual physical system actuator (servo motor or any other valid actuator).

Hope to construct together many or a few most valuable, really utile, useful actor controllers(agents) embeddable low code and high code C/C++ embedded running on digital computer codes for industry. Let's focus on this workflow.

Answer 1

[Mathieu NOE]

hello again

used a simplified version of your model and applied simple PID control (easier for real time conversion) :

% Define system parameters
K = 1;  % Gain
T = 0.1;  % Time constant
L = 0.2;  % Delay

% Transfer function of the first-order system with delay
s = tf('s');
sys_delayed = exp(-L*s)*tf(K, [T, 1]);



% Define PID controller gains
Kp = 0.6;
Ki = 5*Kp;
Kd = 0;

% Transfer function of the PID controller
pid = tf([Kd, Kp, Ki], [1, 0]);

% Closed-loop system
closed_loop_sys = feedback(pid*sys_delayed,1);

% Step response
t = linspace(0, 5, 1000);
[yOL, t] = step(sys_delayed, t);
[yCL, t] = step(closed_loop_sys, t);

% Plot the step response
plot(t, yOL,'b',t, yCL,'r')
xlabel('Time (s)')
ylabel('Response')
title('Step Response of First-Order System with Delay and PID Controller')
grid on

[Mathieu NOE]

a further attemp to optimize the PI controller using pidtune

% Define system parameters
K = 1;  % Gain
T = 0.1;  % Time constant
L = 0.2;  % Delay
% Transfer function of the first-order system with delay
s = tf('s');
sys_delayed = exp(-L*s)*tf(K, [T, 1]);

% Define PID controller gains
[pid,info] = pidtune(sys_delayed,'PI');

% Closed-loop system
closed_loop_sys = feedback(pid*sys_delayed,1);
% Step response
t = linspace(0, 5, 1000);
[yOL, t] = step(sys_delayed, t);
[yCL, t] = step(closed_loop_sys, t);
% Plot the step response
plot(t, yOL,'b',t, yCL,'r')
xlabel('Time (s)')
ylabel('Response')
title('Step Response of First-Order System with Delay and PID Controller')
grid on

[Pavl M.]

that is too simple, trivial, simplification often makes losses in features and original beauty of complexity, make it simple, but not simpler. In this question case the problem is another, it is of proper LQ controller design (the wise cost matrices Q,R,N selection and state estimation or better recovery LTM) and LQ-derivatives compare-contrast and most fitted selection implementable on embedded board must be original the 5+ size A,B,C,D, matrices and not what you/others pushed, also the delay and amplitude defficiency they did not tackle, while I handle in my answers/solution pathes, LQ may be better in performance than PID since it inherently deals with convex optimization and I found the gain K matrix for reduced overshoot, faster settling, more close to desired gain and lower rise time (and so less sluggish) as per the original requirements. Hire me for better, including block diagrams/block schemes and flow of control with TCE Simulink I can make normal.

[Mathieu NOE]

well the art of engineering is to choose the right tool for the right job. I have nothing against the beauty of complexity, but afetr many yeras in the industry, 99.9% of the time I prefer to simplify the problem first before designing a solution for it. of course , it's everyone personal choice / preferences to pick this or that appoach.

but when it comes down to embedded software, the complexier your design the more headaches to test and make sure sure there is no hidden bug somewhere.

Answer 2

[Pavl M.]

It is indeed really factually very valued, yet undervalued and quite intersting, utile question.

(Updated, 17 Nov. 2024, Final for the numerical LQR, LQI gains design):

Here is a solution for much more faster LQ controller(agent) working in loop with reduced oscillations by pre-filter I developed(constructed) novel (plagiarism free, not taken from any published manuscript, further if will be found interested engineering specialist to hire me I can analyze and make loop transfer recovery (LTR) and LTM agents(regulators,controllers) and tackle other sophisticated and simple looped systems, whatever indeed works in soft real time embedded). What is better for you LQR(no augmentation of matrices A,B,C,D, is required) or LQI(needs augmentation of A,B,C,D, matrices)

clc
clear all
close all

s_time = 0.02; %[sec], SI by default in TCE Matlab
Np = 0.01; %noise power
nstates = 5;
ninputs = 1;
ncontrols = 1;
noutputs = 1;
% System identification
A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;
    0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
    -0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
    0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
    -0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];
B = [-0.0691336950664988;
    0.298054008136356;
    -0.101282668881553;
    -0.135571075515549;
    -0.197136001368224];
C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];
D = 0;

sys1 = ss(A, B, C, D, s_time);
sys2 = ss(A,B,C,D);

disp('Whether the system is stable, minimum phase and proper:')

Whether the system is stable, minimum phase and proper:

isstable(sys1)

ans = logical
   1

isminphase(tfdata(tf(sys1),'v'))

ans = logical
   0

isproper(sys1)

ans = logical
   1

Qc= ctrb(A,B)

Qc = 5×5

-0.0691 -0.0919 -0.0764 -0.0676 -0.0674 0.2981 0.2670 0.2051 0.1271 0.0493 -0.1013 -0.0129 0.0803 0.1345 0.1429 -0.1356 -0.1307 -0.1495 -0.1761 -0.2023 -0.1971 0.0018 0.0625 0.0646 0.0486

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

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

controllab = rank(Qc)

controllab = 5

x = 1;
disp('The pair (A,B) must be stabilizable')

The pair (A,B) must be stabilizable

ic = [0 0 0 0 0];
figure
step(sys1);

Gd1 = c2d(sys2,s_time,'impulse')

Gd1 =
 
  A = 
               x1          x2          x3          x4          x5
   x1       1.015   -0.001577   -0.005541   -0.001399    0.005536
   x2   0.0005412       1.016   -0.004958    0.001511   -0.001557
   x3  -0.0008271    0.007327       1.014    0.001702    0.004031
   x4      0.0018    -0.00107   -0.001754        1.02   -0.001585
   x5  -3.619e-05     0.00293   -0.003634   -0.003176       1.008
 
  B = 
              u1
   x1   -0.00142
   x2   0.006069
   x3   -0.00203
   x4  -0.002764
   x5  -0.003942
 
  C = 
             x1        x2        x3        x4        x5
   y1    0.3955   0.08747    0.1275  -0.05117  -0.04206
 
  D = 
              u1
   y1  2.088e-05
 
Sample time: 0.02 seconds
Discrete-time state-space model.

Gd2 = d2d(sys1,s_time,'tustin')

Gd2 =
 
  A = 
              x1         x2         x3         x4         x5
   x1     0.7482   -0.07708    -0.2728   -0.06803      0.274
   x2    0.02648     0.8024    -0.2442    0.07431   -0.07642
   x3   -0.04092     0.3606     0.7152     0.0837     0.1997
   x4    0.08844   -0.05208   -0.08624     0.9825   -0.07825
   x5  -0.001762     0.1453    -0.1795    -0.1566     0.4109
 
  B = 
             u1
   x1  -0.06913
   x2    0.2981
   x3   -0.1013
   x4   -0.1356
   x5   -0.1971
 
  C = 
             x1        x2        x3        x4        x5
   y1    0.3955   0.08747    0.1275  -0.05117  -0.04206
 
  D = 
       u1
   y1   0
 
Sample time: 0.02 seconds
Discrete-time state-space model.

%figure
%step(Gd1)
figure
step(Gd2)

[A_size_row, A_size_column] = size(A);
Q = 70*eye(A_size_row+1)

Q = 6×6

70 0 0 0 0 0 0 70 0 0 0 0 0 0 70 0 0 0 0 0 0 70 0 0 0 0 0 0 70 0 0 0 0 0 0 70

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

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

R = 7 %*eye(ninputs);

R = 7

R2 = 5 %*eye(ninputs+1);

R2 = 5

N = ones(nstates,1)

N = 5×1

1 1 1 1 1

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

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

N2 = ones(nstates+1,1)

N2 = 6×1

1 1 1 1 1 1

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

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

%S
%E
%P %matrices to find furthermore.
A_aug = [A, zeros(A_size_column, 1); -C, 0];
B_aug = [B; 0];
Q2 = 30*eye(A_size_row) % To be selected using Adaptive Dynamic Programming or 

Q2 = 5×5

30 0 0 0 0 0 30 0 0 0 0 0 30 0 0 0 0 0 30 0 0 0 0 0 30

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

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

%similar heuristic for rubustness
%R = 5;
%S = eye(nstates,noutputs);
%E = eye(nstates,nstates);
%[~,p] = chol(R)
%% Just ARE
[X1, L1, G1] = care(A_aug, B_aug, Q, [1]) %, S, E)

X1 = 6×6

1.0e+07 * 0.3282 -0.1878 -0.3501 -0.5924 0.1873 -0.0026 -0.1878 0.1265 0.2121 0.3805 -0.1127 0.0010 -0.3501 0.2121 0.3817 0.6571 -0.2033 0.0024 -0.5924 0.3805 0.6571 1.1607 -0.3502 0.0037 0.1873 -0.1127 -0.2033 -0.3502 0.1086 -0.0013 -0.0026 0.0010 0.0024 0.0037 -0.0013 0.0001

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

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

L1 = 
  -0.1512 + 0.0000i
  -0.6857 + 0.0000i
  -0.7866 + 0.2586i
  -0.7866 - 0.2586i
  -0.8789 + 0.0000i
  -3.3905 + 0.0000i

G1 = 1×6

1.0e+03 * 2.0157 -1.7673 -2.4339 -5.1361 1.3512 -0.0084

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

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

%% For Just LQR:
[K1,S1,P1] = lqr(sys1,Q2,R)

K1 = 1×5

-0.5461 1.3958 0.1922 -1.2207 -0.1025

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

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

S1 = 5×5

77.7954 -18.7303 -48.1839 34.8112 2.5513 -18.7303 111.1492 33.6975 11.7969 10.2405 -48.1839 33.6975 114.2713 -47.1588 7.3914 34.8112 11.7969 -47.1588 321.9632 -53.5409 2.5513 10.2405 7.3914 -53.5409 50.5475

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

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

P1 = 
   0.6870 + 0.2420i
   0.6870 - 0.2420i
   0.6773 + 0.0000i
   0.6323 + 0.0000i
   0.3556 + 0.0000i

%% For just LQI:
[Klqi,Slqi,elqi] = lqi(sys1,Q,R2,N2)

Klqi = 1×6

-0.6764 1.8167 0.3253 -1.7091 -0.0709 -1.5924

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

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

Slqi = 6×6

1.0e+04 * 0.0185 -0.0034 -0.0111 0.0054 0.0011 -0.0356 -0.0034 0.0225 0.0073 0.0056 0.0024 0.0050 -0.0111 0.0073 0.0265 -0.0101 0.0016 0.0059 0.0054 0.0056 -0.0101 0.0782 -0.0139 0.0953 0.0011 0.0024 0.0016 -0.0139 0.0122 -0.0258 -0.0356 0.0050 0.0059 0.0953 -0.0258 1.8142

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

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

elqi = 
   0.9959 + 0.0000i
   0.6731 + 0.2256i
   0.6731 - 0.2256i
   0.6576 + 0.0531i
   0.6576 - 0.0531i
   0.2009 + 0.0000i

%In discrete time, lqi computes the integrator output xi 
%using the forward Euler formula
%xi[n+1]=xi[n]+Ts(r[n]−y[n])
%where Ts is the sample time of SYS.
%What if to try trapezoidal and Backward-Euler integration methods?
[K2,S2,P2] = dlqr(A,B,Q2,R,N)

K2 = 1×5

-0.5260 1.3925 0.2548 -1.2030 -0.0769

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

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

S2 = 5×5

79.0469 -19.5596 -47.6365 36.6696 3.5117 -19.5596 106.8111 32.0926 11.8435 8.7892 -47.6365 32.0926 113.3643 -45.6810 7.1108 36.6696 11.8435 -45.6810 324.4745 -51.8259 3.5117 8.7892 7.1108 -51.8259 50.9129

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

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

P2 = 
   0.7224 + 0.1105i
   0.7224 - 0.1105i
   0.6348 + 0.2625i
   0.6348 - 0.2625i
   0.3410 + 0.0000i

%Compare K2 and K1, select which produces better results for your real
%world physical system through H2 = A-B*K2
%% For just LQI the integrator and states gain:
K_x = Klqi(1:5);
K_i = Klqi(6);
dt = s_time;  % time step;
T = 100;  % sim time in [sec]
%% For just LQR: System's Hamiltonian
H = A - B*K1

H = 5×5

0.7104 0.0194 -0.2595 -0.1524 0.2670 0.1892 0.3864 -0.3014 0.4381 -0.0459 -0.0962 0.5019 0.7347 -0.0399 0.1894 0.0144 0.1372 -0.0602 0.8170 -0.0921 -0.1094 0.4205 -0.1416 -0.3972 0.3907

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

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

H2 = A - B*K2

H2 = 5×5

0.7118 0.0192 -0.2552 -0.1512 0.2687 0.1833 0.3874 -0.3201 0.4329 -0.0535 -0.0942 0.5016 0.7410 -0.0381 0.1919 0.0171 0.1367 -0.0517 0.8194 -0.0887 -0.1055 0.4198 -0.1293 -0.3937 0.3958

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

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

%% Now when the optimal quadratic gain matrix K is found for your
%% embedded implementation you will need the plant(environment) under control
%% state vector X estimator (it can be Kalman or another more handy and simple,
%% which =?(additional research question m to answer if stakeholder/customer
%% hires me). Then The control law of your system under LQR control is ultimately
%% u = setpointreferenceinputsignal - K*Xest. Very impressive gain and phase margines if
%% we find as precise Xest-X -> 0 and and also for homogeneous and in-homogeneous cases.
% 
Gn = 1; %you may experiment with different gains
sys3 = ss(H,B,Gn*C,D,s_time);
% Convert state-space model to transfer function
sys_tf  = tf(sys3);           
[n, d]  = tfdata(sys_tf, 'v'); 
% Pre-filter
Gf      = -0.1*tf(d(end), n,s_time)        

Gf =
 
                             0.00808
  -------------------------------------------------------------
  0.001044 z^4 - 0.01182 z^3 + 0.0379 z^2 - 0.05049 z + 0.02531
 
Sample time: 0.02 seconds
Discrete-time transfer function.

% Display the transfer function of the Command Compensated System
disp('Transfer Function of the Command Compensated System:');

Transfer Function of the Command Compensated System:

Gcc = minreal(Gf*sys_tf)        

Gcc =
 
                            0.00808
  -----------------------------------------------------------
  z^5 - 3.039 z^4 + 3.713 z^3 - 2.264 z^2 + 0.6835 z - 0.0808
 
Sample time: 0.02 seconds
Discrete-time transfer function.

ms = minreal(Gcc);
zms = zero(ms)     % closed-loop zeros

zms =

  0x1 empty double column vector

pms = pole(ms)     % closed-loop poles

pms = 
   0.6870 + 0.2420i
   0.6870 - 0.2420i
   0.6773 + 0.0000i
   0.6323 + 0.0000i
   0.3556 + 0.0000i

%Step response of the resultant system to step in amplitude 12 applied at
%time = 1 sec
% replaces loop:
tsim = 10;
setpointamp = 24; %Bar or Newton or mm
setpointapptime = 1; % sec
defaultsetpointpos = 0; % Bar or Newton or mm
Conf = RespConfig(Bias=defaultsetpointpos,Amplitude=setpointamp,Delay=setpointapptime)

Conf = 
  RespConfig with properties:

                Bias: 0
           Amplitude: 24
               Delay: 1
        InitialState: []
    InitialParameter: []

figure
step(Gcc,[0 tsim], Conf)
title('Plant+Controller LQR Closed Loop system step response')

%% PID
P1   = tf(sys1)

P1 =
 
  0.001044 z^4 - 0.01182 z^3 + 0.0379 z^2 - 0.05049 z + 0.02531
  -------------------------------------------------------------
   z^5 - 3.659 z^4 + 5.401 z^3 - 4.011 z^2 + 1.491 z - 0.2201
 
Sample time: 0.02 seconds
Discrete-time transfer function.

[C, info] = pidtune(P1, 'PIDF2')

C =
 
                        Ts  
  u = Kp (b*r-y) + Ki ------ (r-y)
                        z-1 

  with Kp = 0.442, Ki = 2.92, b = 1, Ts = 0.02
 
Sample time: 0.02 seconds
Discrete-time 2-DOF PI controller in parallel form.

info = struct with fields:
                Stable: 1
    CrossoverFrequency: 2.3678
           PhaseMargin: 60.0000

C2tf = tf(C)

C2tf =
 
  From input 1 to output:
  0.4418 z - 0.3835
  -----------------
        z - 1
 
  From input 2 to output:
  -0.4418 z + 0.3835
  ------------------
        z - 1
 
Sample time: 0.02 seconds
Discrete-time transfer function.

Cr = C2tf(1)

Cr =
 
  0.4418 z - 0.3835
  -----------------
        z - 1
 
Sample time: 0.02 seconds
Discrete-time transfer function.

Cy = C2tf(2)

Cy =
 
  -0.4418 z + 0.3835
  ------------------
        z - 1
 
Sample time: 0.02 seconds
Discrete-time transfer function.

Gclp = Cr*feedback(P1,Cy,+1)

Gclp =
 
  0.0004612 z^6 - 0.006086 z^5 + 0.02691 z^4 - 0.05813 z^3 + 0.06739 z^2 - 0.04026 z + 0.009708
  ---------------------------------------------------------------------------------------------
       z^7 - 5.659 z^6 + 13.71 z^5 - 18.45 z^4 + 14.86 z^3 - 7.146 z^2 + 1.891 z - 0.2104
 
Sample time: 0.02 seconds
Discrete-time transfer function.

Gcl = minreal(Gclp);
zcl = zero(Gcl)     % closed-loop zeros

zcl = 
   7.1078 + 0.0000i
   1.3136 + 0.6463i
   1.3136 - 0.6463i
   1.5918 + 0.0000i
   1.0000 + 0.0000i
   0.8680 + 0.0000i

pcl = pole(Gcl)     % closed-loop poles

pcl = 
   0.7309 + 0.3420i
   0.7309 - 0.3420i
   1.0000 + 0.0000i
   0.9419 + 0.0965i
   0.9419 - 0.0965i
   0.9224 + 0.0000i
   0.3906 + 0.0000i

% Plot the results
subplot(3,1,1)
step(sys1,[0 tsim], Conf),       grid on, legend('Original system',    'location', 'east')
subplot(3,1,2)
step(Gcl,[0 tsim], Conf), grid on, legend('2DOFPIDF Compensated system', 'location', 'east')
subplot(3,1,3)
step(Gcc,[0 tsim], Conf), grid on, legend('LQR Controlled system', 'location', 'east')

%full, complete component so far. Clear improvement in magnitude and
%settling time response of the discrete time closed loop under the
%specified control.
% 
 pause

Warning: Pause with no arguments is not supported in non-interactive contexts.

%[K,S,e] = lqry(sys,Q,R,N)
%% More robust gaussian servo controller:
nx = 5;    %Number of states
ny = 5;    %Number of outputs
%To improve the sharpness and settling of the LQG1 step response, you can try adjusting the weighting matrices and controller design parameters. Here are some suggestions to enhance the performance of the KLQG1 controller:
    %Increase the weight on the state variables in the QXU matrix to prioritize the control effort's impact on the system states.
    %Tune the weights in the QWV matrix carefully, especially the one related to the noise covariance (Rn) to reduce its influence on the closed-loop response.
    %Experiment with different values for the QI matrix to adjust the integral control's contribution to the overall controller behavior.
    %Consider refining the controller design by incorporating additional design constraints or performance specifications tailored to your specific requirements.
%By iteratively adjusting these parameters and observing their effects on the closed-loop system response, you can fine-tune the KLQG1 controller to achieve a sharper and more settled step response. 
% Define the different values of Rn to test
Rn_values = [0.01, 0.05, 0.1, 0.5, 1];
P = sys1;
QI = 5.5 * eye(ny);
Qn = Q2; % + 50 * [0.05 0.03 0 0 0; 0.03 0.02 0 0 0; 0 0 0.01 0 0; 0 0 0 0.01 0; 0 0 0 0 0.01];
R = 0.1*sqrt(Qn);
QXU = 30*blkdiag(eye(nx), R);


%Weighting matrices Q, R, and N, which define the tradeoff between regulation performance (how fast x(t) goes to zero) and control effort.
%x = [0;0;0;0;0];
%x_i = x;
%y = [];
%u = [];
%ref = [];
%states = [];
%r=0;
%Todo: simulate it in loop:
%noise = 0.001;
%ToDo next is not yet fully full: not full, under development.
%for t = time
%    if t == 50
%        r = 5;
%    end

%    u_t = -K_x * x - K_i * x_i;

%    u_t(abs(u_t)>abs(max(C))) = max(C);
%    x(abs(x)>abs(max(r))) = r;

%    x_dot = A * x + B' * u_t;
%    xn = x + x_dot * dt;
%    y_t = C * xn + D*u_t;
    %Trapezoidal integrator (ToDo: try other discrete integrators approximations, like backward and trapezoidal)
%    x_i_dot = r - y_t + r - C * x + D*u_t;
%    x_i = x_i + x_i_dot * dt/2;
%    y = [y, y_t];
%    u = [u, u_t];
%    ref = [ref, r];
%    x = xn;
%    states = [states, x];
%end

%figure;
%subplot(3,1,1);
%plot(time, ref, 'r--', 'LineWidth', 1.5);
%hold on;
%plot(time, y, 'b', 'LineWidth', 1.5);
%xlabel('Time (miliseconds)');
%ylabel('Output y');
%title('System Output vs Reference Signal');
%legend('Reference', 'Output');
%grid on;
%subplot(3,1,2);
%plot(time, u, 'g', 'LineWidth', 1.5);
%xlabel('Time (miliseconds)');
%ylabel('Control Effort u');
%title('Control Effort');
%grid on;
%subplot(3,1,3);
%plot(time, states, 'LineWidth', 1.5);
%xlabel('Time (miliseconds)');
%ylabel('States');
%title('States over Time');
%legend('x1', 'x2', 'x3', 'x4', 'x5');
%grid on;

%Option 2, construct positive semidefinite matrices:
%Nn = 1;
%[kest,L,P2] = kalman(sys1,Qn,Rn,Nn);
%Connect the  and sys to form the working loop, it should bring more performant control:
%K4 = lqi(sys1,Q,R)
%servocontroller = lqgtrack(kest, K4);
%C=servocontroller;
%set(C,'InputName','e')
%set(C,'OutputName','u')
%set(P,'OutputName','y')
%set(P,'InputName','u')
%Sum_Error = sumblk('e = r - y');
%clsys2 = minreal(tf(connect(C,P,Sum_Error,'r','y')))

%figure
%step(clsys2)
%title('Constructed closed loop system response 2')

%Option 3:
%C = lqgtrack(kest,K4)
%C = lqgtrack(kest,K4,'2dof')
%C = lqgtrack(kest,K4,'1dof')

%Fcl = minreal(series(P, C));
%serc = minreal(feedback(Fcl,sumblk));

%Constructed from needing help prgm. by
    %https://independent.academia.edu/PMazniker
    %[phone], https://join.skype.com/invite/oXnJhbgys7oW
    %https://diag.net/u/u6r3ondjie0w0l8138bafm095b
    %https://github.com/goodengineer
    %https://orcid.org/0000-0001-8184-8166
    %https://willwork781147312.wordpress.com/portfolio/cp/
    %https://www.youtube.com/channel/UCC__7jMOAHak0MVkUFtmO-w
    %https://nanohub.org/members/130066
    %https://pangian.com/user/hiretoserve/
    %https://substack.com/profile/191772642-paul-m

(here obvious delay reduction demonstrated as well as amplification and 1 loop reduction (step with config instead of for loop))

Simplest design of LQR, LQI solution presented:

because you created discrete time state space model and the lqr with matrices deals with continuous time.

What is the value of N and S, E Ricatty matrices for more precise control(agent actions) for your specific plant(environment)?

[K,S,P] = lqr(sys,Q,R,N) for discrete systems.

[K,S,e] = lqi(SYS,Q,R,N) for discrete systems.

[K,S,P] = dlqr(A,B,Q,R,N) for discrete systems only.

If you putted there matrices (A,B,C,D) it is for continuous systems.

Also the disturbance and noise impact-cost to analyze more and the system can be more tuned (settling time, overshoot, phase and gain margins, response times may be improved).

I plan to continue to elaborate it further to most precise completion. There are a few solutions availble to this.

What do you mean by the plant? What is the real world non-linear time varying (little time varying) initial the plant model? At which point linearized? How did you obtain the A,B,C,D, matrices? Which plant is it exactly (is it servomotor+pedal+the rest of braking system or is it just braking pedal and rest of braking system or just servomotor)?

What is the main goal to control mostly the braking pedal relative displacement (and so 1-to-1 mapping the servo position as the main set point) or braking pedal force excerted to control force applied on pedal or just desired braking pressure (main set-point)?

Should I make the few versions of the real world setup plants for you?

In simulation you are on time, in reality it is discrete time (quantization and sampling impacts are added).

So on discretization.

Correction to your post:" I have neve had any issues with goal reaching".

What are the discrete controller and plant precise parameters as in your real world system?

Correction to your post:

Your wrote in post :"servomechanism LQI controller."

But in code there is LQR controller.

I added both LQR and LQI design for discrete.

Have you tried the LQG control I proposed as more fitted clear in previous your similar post?

Have you tried PID tuning?

So far you made in script. I've just started to ammend, improve your the code script, the system and controller design and preparing of Simulink diagram, block-scheme for it...

Hope to help, can you await till I complete and upload it? Here is good educational summary of available controllers for you(I have found a few more, not yet presented in the TCE Mathworks matlab self-paced curses, which is fPID, LQG,...more I can find/look up and put effort to understand if there will be found stakeholders(customers,clients,invsestors) to order the Project service and products (both digital products and processes contribution which will lead to products and services)

Here I've created novel model and sim with your A,B,C,D, matrices and step input and white noise disturbances in actuator (control agent) output(actions) and plant output (sensor noise) to model it as close to real world and with PID(z) controller with Forward Euler and Trapezoidal Integration and filtered derivative, not saturated, 3 signals(control effort, yellow - plant resultant output, setpoint) presented. As we can see the hope is robust, the plant got stabilized indeed in full with the PID and the output is even over-gained, while delay in output is still up to 1 sec even in the PID control case, which is quite large and for it others use specific sluggish systems treatment techniques, which I think to cover next. The design is modular, hypothethical models also presented in it in case to grow it modular.

[Wiktoria]

Hello!

Thank you for your answer! I have found a research that implemented LQI based on augmenting the matrices so that the integral term is taken into consideration and then just using lqr function. From what I understood, it I augment the matrices so I take the integral into consideration, it is basically LQI. Perhaps that was wrong?

I haven't tried simulink because I have no idea how to convert the code from simulink to embedded (in my case, STM32). Additionally, the servo braking is a part of a bigger project, so I am a bit confused with how to mend it all together.

My plant is basically a bolide (formula student car). I got the state space matrices after obtaining response to step from system (but step was from 0 bars to 24 bars) and then running System Identification Toolbox to estimate state space model. The plant is servomechanism connected to a brake pedal via steel wire and it gets requested brake pressure via CANbus.

The goal is to implement the controller with a good transient response (so no excessive or sharp changes in control signal) and to make it robust, reliable and fast. We have used a PID controller before but - at least from what I got to know - it didn't give satisfactory performance. My task was to implement a different one (like LQR, LQI or something else) to compare the solutions and performence between the new solution and the previous one.

I have tried to implement the LQG controller you have mentioned, but I am confused with the equation the control signal should have. I tried to compute it, looked up on MATLAB guidelines, but when I implemented it, it was unstable, so I tried to fix my current solution. I have tried to compute the u_t using gain counted by dlqr function, as + noise.

Since my goal is to implement a different controller than PID, I haven't tried PID tuning.

My state space model is for sure discrete. I have check it and MATLAB returns that it is a discrete time, sampling 0.02s.

The servomechanism works for pulse lengths from 900 us to 1300 us. In current solution, the controller computes the control signal (as position), remaps it so it's between 0 to 90 degrees and then computes the pulse length for the computed, rescaled position. The delay between the servo receiving the command and reacting to it is around 10 to 20 ms, resolution of the servo is <2us. In embedded system (STM32) servo actuation is set up on 50Hz timer (so every 0.02s). First is checks if there is a need for actuation (by checking if error between goal pressure and current pressure is acceptable) and if it isn't, the controller starts to compute the output and servo moves until it reaches the goal and is within the acceptable error range.

[Pavl M.]

OK, it is doable (both 2DOF fractionalPID and LQ-family of controllers (including LQR-PID hybrids, and loop recovery can be tried on it), when I complete I will upload it:

Loop Recovery techniques are very promissing since allow more accuracy than Kalman X state estimator. Let's check it.

Please check recent update to the initial answer here, fast and robust K gain for LQR controller and 2DOFPID and tracking found. Let it possible I will add more to it.

Similar very related question:

https://www.mathworks.com/matlabcentral/answers/530608-how-do-i-get-my-desired-settling-time-or-rise-time-using-lqr-which-things-i-need-to-change-to-get-n?s_tid=answers_rc1-2_p2_MLT

Carefull selection of Q,R,N matrices are similar in goal vs the process of PID tuning.

Let's see maybe you can use the controller (smart agent for which the good K gains finding we have completed) with your system without complex Particle Filter or Kalman state X estimator if to rely on your matrices A,B,C,D (please check are you sure about the matrices, whether they work in embedded code?) solely then the state X can be computed inside the embedded code from equation

X_dot = A*X+B*U,

then the control output can be computed as:

LQR:

U = (setpoint - y_from_real_phys_system)*Klqr(N+1) - (Klqr1.. KlqrN)*(X1 .. XN) (designed so far with augmented)

U = (setpoint - y_from_real_phys_system)*Klqr(1) - (Klqr2.. KlqrN)*(X2 .. XN)(not augmented)

or U = F1(setpoint - y_from_real_phys_system) - (Klqr1...KlqrN)*(X1..XN)

LQI:

u = DiscreteIntegratorApproximator(r - y_from_real_phys_system)*Klqi(N+1) - Klqi(1..N)*X(1..N) (most precise)

or

u = DiscreteIntegratorApproximator(r - y_from_real_phys_system)*Klqi1 - Klqi(2..N+1)*X(1..N). (depending on augmentation from beginning or augmentation at end)

The control u computed on your embedded board relying on the actual real physical system estimation matrices A,B,C,D, hard coded in your embedded code and using actual physical system output y, the control u computed you just connect via appropriate transducer (signal conditioner) to your actual physical system actuator (servo motor or any other valid actuator).

Let's focus on this workflow.

The control u computed on your embedded board relying on the actual real physical system estimation matrices A,B,C,D, hard coded in your embedded code and using actual physical system output y, the control u computed you just connect via appropriate transducer (signal conditioner) to your actual physical system actuator (servo motor or any other valid actuator).

Let's focus on this workflow for next few monthes and we will see also can we obtain in practice better than PID control with LQR (in theory LQR is better than PID, but whether better than the proposed 2DOFPID, with best phase and gain margins), but in practice due to the X state observer additional design let's see what we get in practice in your embedded board in C,C++ controller running compiled (in mind) (please keep your the setup, I will not steal it, while already today others many stolen from me more locally, that is why I request to find material substance support investor, granter to help only financially by Money Gram, RIA or Western Union remiitance to Pavlo Mazniker) . It is very valued industry and academic research question with many unknowns to commerciallize.

OK, as for the LQ RAE goal conroller(agent), it is feasible of course, for just LQR/LQI it must needs to

design implementable online (real-time) closed loop Kalman state estimator as much accurate and robust, while the K gains we have already found, I have started Simulink diagram blockscheme design of LQ with different Kalmans to select best one and also thinking more robust and more implementable on digital hardware (Which is better MCU, CPLD, FPGA, custom ASIC? for future applications?) than Kalman.

I tried today also to close and run loop with just 1 feedback gain element of value operation_on(K) and in feedforward integrator + the operation_on(K) gain, but yet the output is going down, sign that more complex but not complicated design is on agenda.

The question is that from time to time the LQR-LQG controllers found different interpretations in academic research and whether online(real time) closed loop Kalman gain estimator is required if we already pre-computed the K gains?

We need to distinguish between Kalman state X estimator and Kalman K gain estimator

and 3 main families of Ricatti Optimization LQ Designs:

LQG with Kalman K gain vector estimator and 1 integrator

LQR wich is just I controller from PID tuned using Ricatti Optimization with balance matrices carefully selected as in the rest of the kinds. Here also experiments with vector/matrix of K gains transformation as a feedback gain or

feedforward as I controller or a combination of both can be conducted if you contact and hire me.

LQI is a controller with gain, integrator and Kalman plant internal state(X) estimation

LoopTransferRecovery (LTR) technique,

What are your heuristics to choose Q,R,N matrices?

In previous the PID closed loop Simulink simulation presented, let me correct my the PID for comparison: there should be - in sum block on output signal on feedback, that picture were produced with + sign there.

It can be doable in TCE Simulink of course, have you tried Scicos/Scilab?

Are you sure about 900 to 1300 [us = 0.000001 sec] and not [0.001 sec = ms], please check time units consistency:

please check:

900 us to 1300 us. In current solution, the controller computes the control signal (as position),

remaps it so it's between 0 to 90 degrees and then computes the pulse length for the computed,

rescaled position. The delay between the servo receiving the command and reacting to it is around 10 to 20 ms,

resolution of the servo is <2[us]

Are you sure about <2[us]?

Is the servo reacting delay already resembled in pre-obtained the A,B,C,D matrices (are needed to implement the actual controller(agent) logic in embedded C) or should we model / regard it additionally (as *exp(... delay_value))?

Answer 3

[Paul]

Following up from this comment, we have established that the plant is modeled in disctete-time with sampling period Ts = 0.02 s.

% System identification
A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;
    0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
    -0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
    0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
    -0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];
B = [-0.0691336950664988;
    0.298054008136356;
    -0.101282668881553;
    -0.135571075515549;
    -0.197136001368224];
C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];
D = 0;
Ts = 0.02;
sys = ss(A, B, C, D, Ts);

However, the rest of the code in the Question, i.e., the augmentation, the call to lqr, and the simulation, is treating the model as if its in continuous-time, so that all needs to be corrected.

It appears the objective is to use the LQR technique with integral control, for which we have two options. Option 1 is to manually augment the plant with a discrete-time integrator and then call lqr with the augmented LTI object (not Aaug and Baug), Option 2 is to use lqi and let the software do the work for us.

Let's use option 2.

Q = 70*eye(size(A,1)+1);
R = 5;
K = lqi(sys,Q,R);

Now that we have our gains, we have to build the closed loop system based on those gains. We have some options here. Option 1 is to manually manipulate the models to come up with a closed-form expression. Option 2 is to use connect so we can more intuitively represent the system as a block diagram, and it also provides more flexibility going forward when we want to do other things with the model, like add models for a sensor or specficy breakpoints for stability margins. Let's use option 2

The plant model presently has only one output, y, but we also need to output the state variable for state feedback

sys = ss(A,B,[C;eye(5)],0,Ts,'InputName','u','OutputName',["y", "x("+(1:5)+")"]);

Define the sum block that forms the error signal

errsum = sumblk('e = r - y');

Define the discrete time integerator using the equation shown at lqi

intsys = ss(1,Ts,1,0,Ts,'InputName','e','OutputName','xi');

Define the gains

K_x = ss(K(1:5),'InputName','x','OutputName','ux');
K_i = ss(K(6),'InputName','xi','OutputName','ui');

Sum of the control signals from the state feedback and the integral control

usum = sumblk('u = -ui - ux');

I think you're going to want a stand alone model of the compensator, so let's build that first

syscomp = connect(errsum,intsys,K_x,K_i,usum,["r";"y";K_x.InputName],'u');

Put it all together

syscl = connect(sys,syscomp,'r','y');

Now, for simulation we can either write our own loop, or we can just use step. Compare the reponse of the plant to the closed-loop system

figure
step(sys('y','u'),syscl)

The slow response of the closed loop system is due to the selection of Q and R. I don't know how you came up with those or what your design goals are, so won't suggest how to modify those. I can give you an idea of one approach that might be worth pursuing if you'd like.

[Wiktoria]

Thank you so much for your help! I can fully see now why my approach wasn't good. My main concern though is that I need to develop the loop by myself, since the code will be rewritten to C. I wanted to create a loop step by step, like it is done in my (now wrongly implemented) code. Additionally, the Q and R matrices are choosen incorrectly - they used to work better with my previous solution, but now they require changing. I plan on using the Bryson's rule to determine those. My goal is to implement a fast and reliable system without much change in the control signal.

I will be thankful for any ideas and I'll gladly learn about a new approach I can use. Thank you!

[Paul]

For selecting Q and R, Bryson's rule might be a good starting point if you have a good understanding of the physical interpreration of the state variables, I suppose.

Here's another approach you can experiment with.

% System identification
A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;
    0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
    -0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
    0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
    -0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];
B = [-0.0691336950664988;
    0.298054008136356;
    -0.101282668881553;
    -0.135571075515549;
    -0.197136001368224];
C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];
D = 0;
Ts = 0.02;
sys = ss(A, B, C, D, Ts);
zpk(sys)

ans =
 
     0.0010438 (z-7.108) (z-1.592) (z^2 - 2.627z + 2.143)
  ----------------------------------------------------------
  (z-0.4701) (z^2 - 1.708z + 0.7385) (z^2 - 1.482z + 0.6339)
 
Sample time: 0.02 seconds
Discrete-time zero/pole/gain model.

Define the quadratic cost to be minized as: H = a*y^2 + b*yi^2, where y is the output of the plant and yi is integral of y, whics is the integrator state in the control, xi. Then, using the fact that y = C*x, we have

H = a*x' * C' * C * x + b*xi^2,

which can be combined as

a = 10; b = 500;
Q = blkdiag(a*C'*C,b);
R = 1;  % everything scales with R, so just set it to one
K = lqi(sys,Q,R);

sys = ss(A,B,[C;eye(5)],0,Ts,'InputName','u','OutputName',["y", "x("+(1:5)+")"]);
errsum = sumblk('e = r - y');
intsys = ss(1,Ts,1,0,Ts,'InputName','e','OutputName','xi');
K_x = ss(K(1:5),'InputName','x','OutputName','ux');
K_i = ss(K(6),'InputName','xi','OutputName','ui');
usum = sumblk('u = -ui - ux');
syscomp = connect(errsum,intsys,K_x,K_i,usum,["r";"y";K_x.InputName],'u');
syscl = connect(sys,syscomp,'r','y');
figure
step(sys('y','u'),syscl)

I just played around a bit with a and b to get a reasonable looking response. Not sure what you're looking for, but with this approach there are only two parameters to iterate over to try to meet the design objectives.

There are probably other methods for selecting Q and R.

As for simulating everything in a loop, you have the difference equations of the controller and the plant as defined by their state space models that you can iterate. But, from what I understand of your project you really only need to implement syscomp in C code.

Also, I'm wondering about the plant model. It kind of looks like the plant has about 10 cycles of delay, which is somehow captured in the A/B/C/D dynamics. Maybe the system identification should try capture the delays explicitly. As it stands now, it's possible that you won't have all of the states of the identified system available for feedback, but I don't really know what your system is or what your modeling, so my concerns could be unfounded.

[Wiktoria]

To be honest, I actually need the loop written step by step like in the code I provided, since it makes it easier for me to understand as well as I have no idea how to switch from MATLAB to embedded C when I have so many functions used. Like I mentioned, I am a beginner and although I find control theory fascinating, it sure is a lot to unfold.

In reality just a subsystem supposed to control the servo - all I receive is the goal pressure I need to achieve and I have to control the servomechanism accordingly. Goal pressure is set via CAN. I have heard that there might be a small delay caused by both the information transfer and servo reaction, but heard not to worry as of now. I will think about trying to capture the delays in model explicitly though, I believe I can do that in System Identification Toolbox.

Thank you for all your help! I am really grateful for the explanation you provided.

[Mathieu NOE]

@Wiktoria

Hi Wiktoria,

could you share the data for the system identification

we need both the stimulus (input ) and output

I still wonder how we get the 200 ms delay

have a great day

[Pavl M.]

@Wiktoria,

From where they took next quadratic cost:

H = a*y^2 + b*yi^2

How it correlates with J and Ricatti initial LQ control definitions.Mainly to select the cost matrices and understand where the delay in your plant(process under control,environment of the braking path) comes, whether conversions or plant subsystem introduced it or the problem in controller?

By the agent(controller) to reduce delay by reducing t_rise(rise time) and settling_time is already not bad, and not to loose in gain, while to reduce control effort how phase and gain margins performance characteristics resemble the latency reduction/quantity?

They used before zero-poles map, root locus and bode plot to adjust the weighting matrices, mainly state cost Q each cell separately as per physical sense on the X(1:i) states, cross matrix N, control cost R state-control, interstate, noise covariance, disturbance rejection vs setpoint tracking

To compute the LQG regulator, lqg uses the commands lqr and kalman. To compute the servo-controller, lqg uses the commands lqi and kalman.

LQG is good, because it produces full controller (both LQ and state estimation) in 1 just 1 completed block and takes into considerations also the plant state noise. But so far i just found it running suboptimally. For more optimal performance there will be needing more experiments with QXU QWV QI matrices.

Another good implementation roadmap is that for simplicity of realization on embedded board you may not be needing Kalman state observer without loss of control efficicency? In so case the PID and LQR with plant states estimated from the X of your system identification efforts A B C D matrices are the best fit.

You need to take into account also usually very harsh conditions of automotive systems in real life

and so MPC with robust tracker will be needed to SOA.

Right from the start in order to alleviate painfull trial-and-error iterative design procedure, a few pre-developed, pre-trained heuristics can be used:

a. Understand your control effort energy demands/limitations, your physical system model natural sens, especially the states X sense;

b. accurate1-to-1 connecting expressions (mapping morphism) between output characteristics (such as rise time, settling time, overshoot, undershoot, stability margins, gain margins, frequency response ,damping, disturbance rejection power, control effort power) and controller design parameters;

c. Closed loop architectural constraints;

If you need more specific R&D on this domain contact me more in private.

To find also: det(sI − (A − BK)) would be good to find how rise time can be expressed by Q,R,N, K is

a novel research path(topic, theme).

Please see update to the answer, whether next LQG1 and LQG2 controllers (The proposed controllers are still yet simple!) with the QXU, QWV, QI matrices selection process is similar to Q,R,N matrices (already includes in it Kalman state estimator) produce less delay faster and more optimal settlement?:

%% More robust gaussian servo controller:
% Define the different values of Rn to test
Rn_values = [0.01, 1];
P = sys1;
% Loop through each value of Rn
for i = 1:length(Rn_values)
    
    Rn = Rn_values(i);
    
    % Tune the weighting matrices
    Qn = Q2 + 50 * [0.05 0.03 0 0 0; 0.03 0.02 0 0 0; 0 0 0.01 0 0; 0 0 0 0.01 0; 0 0 0 0 0.01];
    R = sqrt(Qn);
    QXU = blkdiag(eye(nx), R);
    QWV = blkdiag(Qn, Rn);
    QI = 0.5;
    
    % Redesign the KLQG1 and KLQG2 controllers
    [KLQG1,info1] = lqg(sys1, QXU(1:A_size_row+1, 1:A_size_column+1), QWV, QI, '1dof');
    [KLQG2,info2] = lqg(sys1, QXU(1:A_size_row+1, 1:A_size_column+1), QWV, QI, '2dof');
    
    % Construct the closed-loop systems
    C = KLQG1;
    set(C, 'InputName', 'e');
    set(C, 'OutputName', 'u');
    set(P, 'OutputName', 'y');
    set(P, 'InputName', 'u');
    Sum_Error = sumblk('e = r - y');
    clsys = tf(connect(C, P, Sum_Error, 'r', 'y'));
    clsys_m = minreal(clsys);
    
    C = KLQG2;
    set(C, 'InputName', 'e');
    set(C, 'OutputName', 'u');
    set(P, 'OutputName', 'y');
    set(P, 'InputName', 'u');
    Sum_Error2 = sumblk('e = r - y');
    clsys2 = tf(connect(C, P, Sum_Error2, 'r', 'y'));
    clsys_m2 = minreal(clsys2);
    
    % Plot the results
    figure;
    subplot(2, 1, 1);
    step(clsys_m, [0 tsim], Conf);
    title(['KLQG1 Closed-Loop System Response (Rn = ', num2str(Rn), ')']);
    
    subplot(2, 1, 2);
    step(clsys_m2, [0 tsim], Conf);
    title(['KLQG2 Closed-Loop System Response (Rn = ', num2str(Rn), ')']);
    
    % Pause to allow the plots to be viewed
   
end

(simpler controllers)

Will next function help you to estimate more accurately the state space model of your plant/environment/process to control:

https://www.mathworks.com/help/ident/ref/ssest.html

sys = ssest(data,nx) estimates a continuous-time state-space model using the time-domain or frequency-domain data in the data object data.

Then you can get more data for your A,B,C,D matrices, ushc us nx = 5 or 4 or more.

What compare, predict and sim TCE functions produced for you identified system?

Which is optimal nx (number of states) for your the environment/plant/process model?

Use this syntax especially when you want to estimate a state-space model using frequency-domain or frequency-response data, or when you want to take advantage of the additional information, such as intersample behavior, data sample time, or experiment labeling, that data objects provide.

Non-linearities can be modeled as:

It is quite very valuable and interesting question. This control is centralized, but when we think as Ph.D. taking into account recent vehicular systems Zonal Controllers (ZCU) how to control it indeed. Also we can think more about the decentralized control: see https://en.wikipedia.org/wiki/Witsenhausen%27s_counterexample.

To be most precise the complex system of servomechanism(which is itself actuator with encoder, driver and controller)(mostly linear) then brake pedal path (that wire and brake pedal flange)-> braking cyllinder,pneumo-hydro br. tubes, lines, hoses -> brake disks/drums/pads on wheel hub(most of non-linearities) has non-linearities in it (while if it is correctly reliably constructed it is time invariant) which particular states we can clearly distinguish in it so that it is needed to think also whether QQR, PQR will be required or can we use MPC for necessary wide range of wheel forces and braking system reaction on evolving A,B,C,D linearized system matrices (each time change linearization region and reapeating the LQ(I,R or G) control)? What is the region where the actual physical system is linearized?

[Pavl M.]

clc
clear all
close all

s_time = 0.02; %[sec], SI by default in TCE Matlab

Np = 0.01; %noise power
nstates = 5;
ninputs = 1;
ncontrols = 1;
noutputs = 1;

% Initial(original) datum posted to create agent-environment closed loop:

A = [0.748153090242338 -0.0770817542371630 -0.272770107988345 -0.0680287168856025 0.274040229219026;  
0.0264773037738115 0.802405697397900 -0.244152268701560 0.0743061296316669 -0.0764153326240192;
-0.0409228451430832 0.360565865830120 0.715237466131514 0.0837023187613815 0.199733910116335;
0.0884449413535669 -0.0520801836735802 -0.0862407832241391 0.982500599129848 -0.0782458817344473;
-0.00176167556838596 0.145291619650476 -0.179472954855322 -0.156570713297809 0.410924513958431];

B = [-0.0691336950664988;   
0.298054008136356;
-0.101282668881553;
-0.135571075515549;
-0.197136001368224];

C = [0.395474090810162 0.0874731850181924 0.127527899108947 -0.0511706805106379 -0.0420611557807524];
D = 0;
sys1 = ss(A, B, C, D, s_time);
[A_size_row, A_size_column] = size(A);
Q = 7*eye(A_size_row+1)

Q = 6×6

7 0 0 0 0 0 0 7 0 0 0 0 0 0 7 0 0 0 0 0 0 7 0 0 0 0 0 0 7 0 0 0 0 0 0 7

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

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

Q2 = 50*eye(nstates) % Optimal Q matrix

Q2 = 5×5

50 0 0 0 0 0 50 0 0 0 0 0 50 0 0 0 0 0 50 0 0 0 0 0 50

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

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

R = 0.002*sqrt(Q2) % R matrix

R = 5×5

0.0141 0 0 0 0 0 0.0141 0 0 0 0 0 0.0141 0 0 0 0 0 0.0141 0 0 0 0 0 0.0141

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

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

QXU = Q %30*blkdiag(eye(nstates), R); % Weighting matrix QXU

QXU = 6×6

7 0 0 0 0 0 0 7 0 0 0 0 0 0 7 0 0 0 0 0 0 7 0 0 0 0 0 0 7 0 0 0 0 0 0 7

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

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

% Loop through each value of Rn
Rn = 0.0005 % Optimal Rn value

Rn = 5.0000e-04

Qn = 344*Q2 

Qn = 5×5

17200 0 0 0 0 0 17200 0 0 0 0 0 17200 0 0 0 0 0 17200 0 0 0 0 0 17200

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

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

QWV = 390*blkdiag(Qn, Rn)/1e+06 % Weighting matrix QWV

QWV = 6×6

6.7080 0 0 0 0 0 0 6.7080 0 0 0 0 0 0 6.7080 0 0 0 0 0 0 6.7080 0 0 0 0 0 0 6.7080 0 0 0 0 0 0 0.0000

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

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

QI = 310*eye(noutputs) % Weighting matrix QI

QI = 310

% Redesign the KLQG1 controller with optimal values
KLQG1 = lqg(sys1, QXU, QWV, QI, '1dof');
% Construct the closed-loop system
C = KLQG1;
set(C, 'InputName', 'e');
set(C, 'OutputName', 'u');
set(sys1, 'OutputName', 'y');
set(sys1, 'InputName', 'u');
Sum_Error = sumblk('e = r - y');
clsys = tf(connect(C, sys1, Sum_Error, 'r', 'y'));
clsys_m = minreal(clsys);
% Plot the results
tsim = 10;
setpointamp = 24; % Setpoint amplitude
setpointapptime = 1; % Setpoint application time
defaultsetpointpos = 0; % Default setpoint position
Conf = RespConfig(Bias=defaultsetpointpos,Amplitude=setpointamp,Delay=setpointapptime);
figure;
step(clsys_m, [0 tsim], Conf);
title('Optimized LQG1 Closed-Loop System Response');