# Full state feedback with integral action - Matlab script

I made this Matlab script to implement a dynamical system with full state feedback and integral action.

Maybe I'm wrong in implementing the integral action because the system diverges.

First I converted the continuous time system into discrete time. Then I extended the state and computed the gain to implement full state feedback and integral action. Probably there is an error in how the integral action is implemented in for loop.

Shouldn't "integral in discrete time" simply be the previous sample?

clear;
close all;
clc;

A = [ -0.313   56.7  0
-0.0139 -0.426  0
0      56.7  0];

B = [0.232
0.0203
0];

C = [0 0 1];

%conversion to descrete time

T = .1;

sys = ss(A, B, C, 0);

sysd = c2d(sys, T);

Bd = sysd.B;

Cd = sysd.C;

%extension

A_ext = [ Ad [0 0 0]'
-Cd    1   ];

B_ext = [Bd
0];

%desidered poles
p_des = [0.5 0.501 0.502 0.503];

K = -place(A_ext, B_ext, p_des);

Kr = K(1:3);
Ki = K(4);

N = 100;

%desidered output
yd = 0.05;

x(:, 1) = [0 0 0]';

u(:, 1) = yd * Ki + Kr * x(:, 1); %(yd - 0) * Ki + Kr * x(:, 1)
x(:, 2) = Ad * x(:, 1) + Bd * u(:, 1);
y(:, 1) = Cd * x(:, 1);

for i=2:N

u(:, i) = (yd - y(:, i - 1)) * Ki + Kr * x(:, i);

if (i < N)
x(:, i + 1) = Ad * x(:,i) + Bd * u(:, i);
end
y(:, i) = Cd * x(:, i);

end

k = 1:N;
plot(k, x');



Any help would be appreciated.

Thank you

• "Shouldn't "integral in discrete time" simply be the previous sample?". No. The last row of A_ext clearly shows that the integral is the sum of the previous integrator state and current integrator input (y_d-y). I think, that the accumulation equation corresponding to the last row of the A_ext matrix is not implemented in the for loop.
– AJN
Feb 6 at 15:05
• And how could it be implemented in the for loop? In simulink I simply used the discrete integrator block but how could it be done in command line?
– Alex
Feb 6 at 16:32
• First and foremost, do not "clear" "clear all" every time you start work. That's pointless and potentially desctructive. Next, learn the difference between a macro and a function. It's important. And, don't waste time and memory space renaming field elements ( Ad = sysd.A) It does nothing useful. Feb 7 at 13:45
• You made a common but horrible design error: inside a for loop, that "if" statement is inappropriate and time-wasting. Shift the index down and remove teh "if" Feb 7 at 13:48

The last row of the extended A matrix corresponds to the equation

$$x_{\mathrm{integrator}}(i+1) = -C_d x_{\mathrm{rest}}(i) + x_{\mathrm{integrator}}(i) = -y(i) + x_{\mathrm{integrator}}(i)$$

You can use the extended state and matrices inside the for loop as such. Alternatively, if you decide to use the original state matrices inside the for loop as you have done in the sample code, you need to implement the above equation also in the for loop. It will look something like.

x_int_cur = 0

for i = ...
...

% equation corresponding to last row of the
% extended matrix implemented separately
x_int_cur = (yd - Cd * x(:, i)) + x_int_cur

u(:, i) = Kr * x(:, i) + x_int_cur * Ki;

% optional if you want to plot
% the state of the integrator with respect to time later on.
x_int_total_history(:, i) = x_int_cur;

...
end


Note that, in your sample code, you have used the previous output y(i-1) to feed the integrator. This effectively creates additional states in the system and doesn't match the extended state matrix which uses the current output to feed its integrator.

Also, note that I have not fully checked the modified code above to see if it is exactly matching the extended state matrices. Use it only as a starting point. The main idea is that you have to implement the same extended system equations which you used to place the poles in the for loop also.

• Thank you! It works
– Alex
Feb 7 at 12:54