Full state feedback controller with I servomechanism

Question

Martin Maier on 25 Dec 2019

0
Link

Direct link to this question

https://au.mathworks.com/matlabcentral/answers/497991-full-state-feedback-controller-with-i-servomechanism

Edited: Pavl M. on 6 Dec 2024

I'm trying to calculate the gain matrix K for a full state feedback controller with an I servomechanism. The problem in my calculation is, that with the calculated K the steady state error is really big. But the ss-error should be 0 due to the I servomechanism. I also calculated the K with the place() function but this yields to the same result.

Do anybody know any possible reasons for that?

A = [0 1;
    -0.05 0.4];
B = [0;
    5.886];
C = [1 0];
D = [0];
sys_shifted = ss(A,B,C,D);
%% Transform the System from Continous to Discrete
Ts = 10e-3;       % Samplingtime in s
sysd_ol = c2d(sys_shifted,Ts);
%% Define Discrete Poles  
% poles for the desired damping ratio and natural frequenzy
p(1) = exp((Realpart + Imagpart*1i)*Ts);   
p(2) = exp((Realpart - Imagpart*1i)*Ts);
% pole for the servomechanism
p(3) = exp(-800*Ts);    % place somewhere far left on the left half plane
%% Adopte System for Controller and Servomechanism Desgin
A_ol = [sysd_ol.A(1,1) sysd_ol.A(1,2) 0;
    sysd_ol.A(2,1) sysd_ol.A(2,2) 0;
    -sysd_ol.C(1,1) sysd_ol.C(1,2) 0];
B_ol = [sysd_ol.B(1,1) 0;
    sysd_ol.B(2,1) 0;
    0 1];
C_ol = [sysd_ol.C(1,1) sysd_ol.C(1,2) 0];
D_ol = [0];
%% Calculate Transform Matrix and calculate the control canonical form
%Determine the system characteristic polynomial
CharPoly = poly(A_ol);
% Extract the a0,a1,a2
a0 = CharPoly(4);
a1 = CharPoly(3);
a2 = CharPoly(2);
% Creat inverse Controllability matrix
Pinv_ccf = [a1 a2 1;
            a2 1 0;
            1 0 0];
        
% Calculate Transform Matrix
P = [B_ol(:,1) A_ol*B_ol(:,1) A_ol^2*B_ol(:,1)]; 
Tccf = P * Pinv_ccf;
 
% Calculate Accf, Bccf, Cccf, Dccf
Accf = inv(Tccf)*A_ol*Tccf;
Bccf = inv(Tccf)*B_ol;
Cccf = C_ol * Tccf;
Dccf = D_ol;
 
%% Calculate The Alphas 
ppoly = poly(p); 
alpha0 = ppoly(4);
alpha1 = ppoly(3);
alpha2 = ppoly(2);
%% Calculate Kccf and K 
Kccf = [alpha0-a0 alpha1-a1 alpha2-a2];
K_all = Kccf*inv(Tccf);
%% System for Full State Feedback with Servomechanism 
A_cl = A_ol - B_ol(:,1) * K_all;
sysc_fsf = ss(A_cl,B_ol(:,2),C_ol,D_ol,Ts);

0 Comments
Show -2 older commentsHide -2 older comments

Sign in to comment.

Sign in to answer this question.

Answer 1

Pavl M. on 5 Dec 2024

0
Link

Direct link to this answer

https://au.mathworks.com/matlabcentral/answers/497991-full-state-feedback-controller-with-i-servomechanism#answer_1554004

Edited: Pavl M. on 6 Dec 2024

Open in MATLAB Online

% Answer, full results - stable, minimum phase, proper closed loop system created

clc

clear all

close all

A = [0 1;

-0.05 0.4];

B = [0;

5.886];

C = [1 0];

D = [0];

sys_shifted = ss(A,B,C,D);

%% Transform the System from Continous to Discrete

Ts = 10e-3; % Samplingtime in s

sysd_ol = c2d(sys_shifted,Ts);

%% Define Discrete Poles

Realpart = 1000;

Imagpart = 2000;

% poles for the desired damping ratio and natural frequenzy

p(1) = exp((Realpart + Imagpart*1i)*Ts);

p(2) = exp((Realpart - Imagpart*1i)*Ts);

% pole for the servomechanism

p(3) = exp(-800*Ts); % place somewhere far left on the left half plane

%% Adopte System for Controller and Servomechanism Desgin

A_ol = [sysd_ol.A(1,1) sysd_ol.A(1,2) 0;

sysd_ol.A(2,1) sysd_ol.A(2,2) 0;

-sysd_ol.C(1,1) sysd_ol.C(1,2) 0];

B_ol = [sysd_ol.B(1,1) 0;

sysd_ol.B(2,1) 0;

0 1];

C_ol = [sysd_ol.C(1,1) sysd_ol.C(1,2) 0];

D_ol = [0];

%% Calculate Transform Matrix and calculate the control canonical form

%Determine the system characteristic polynomial

CharPoly = poly(A_ol);

% Extract the a0,a1,a2

a0 = CharPoly(4);

a1 = CharPoly(3);

a2 = CharPoly(2);

% Creat inverse Controllability matrix

Pinv_ccf = [a1 a2 1;

a2 1 0;

1 0 0];

% Calculate Transform Matrix

P = [B_ol(:,1) A_ol*B_ol(:,1) A_ol^2*B_ol(:,1)];

Tccf = P * Pinv_ccf;

% Calculate Accf, Bccf, Cccf, Dccf

Accf = inv(Tccf)*A_ol*Tccf;

Bccf = inv(Tccf)*B_ol;

Cccf = C_ol * Tccf;

Dccf = D_ol;

%% Calculate The Alphas

ppoly = poly(p);

alpha0 = ppoly(4);

alpha1 = ppoly(3);

alpha2 = ppoly(2);

%% Calculate Kccf and K

Kccf = [alpha0-a0 alpha1-a1 alpha2-a2];

K_all = Kccf*inv(Tccf);

%% System for Full State Feedback with Servomechanism

A_cl = A_ol - B_ol(:,1) * K_all;

sysc_fsf = ss(A_cl,B_ol(:,2),C_ol,D_ol,Ts);

figure

step(sysc_fsf,[0 5])

title('System in question before treatment')

res_agent = sysc_fsf

res_agent = A = x1 x2 x3 x1 -2.425e+08 1.212e+06 -1.625e+05 x2 -4.853e+10 2.425e+08 -3.253e+07 x3 -1 0 0 B = u1 x1 0 x2 0 x3 1 C = x1 x2 x3 y1 1 0 0 D = u1 y1 0 Sample time: 0.01 seconds Discrete-time state-space model.

figure

w = logspace(2,6,1000);

bode(res_agent,w),grid

figure

rlocus(res_agent)

figure

nyquist(res_agent)

figure

nichols(res_agent)

Gcc = minreal(res_agent)

Gcc = A = x1 x2 x3 x1 -2.425e+08 1.212e+06 -1.625e+05 x2 -4.853e+10 2.425e+08 -3.253e+07 x3 -1 0 0 B = u1 x1 0 x2 0 x3 1 C = x1 x2 x3 y1 1 0 0 D = u1 y1 0 Sample time: 0.01 seconds Discrete-time state-space model.

ms = minreal(Gcc);

zms = zero(ms) % closed-loop zeros

zms = -1.0013

pms = pole(ms) % closed-loop poles

pms =

1.0e+04 * 0.8989 + 2.0109i 0.8989 - 2.0109i 0.0000 + 0.0000i

tsim = 1;

setpointamp = 1; %

setpointapptime = 0.001; %

defaultsetpointpos = 0; %

Conf = RespConfig(Bias=defaultsetpointpos,Amplitude=setpointamp,Delay=setpointapptime)

Conf =

RespConfig with properties: Bias: 0 Amplitude: 1 Delay: 1.0000e-03 InitialState: [] InitialParameter: []

[wngcc,zetagcc,pgcc] = damp(Gcc)

wngcc = 3×1

1.0e+03 * 0.8000 1.0066 1.0066

zetagcc = 3×1

1.0000 -0.9934 -0.9934

pgcc =

1.0e+04 * 0.0000 + 0.0000i 0.8989 + 2.0109i 0.8989 - 2.0109i

sigma(Gcc)

margin(Gcc)

diskmargin(Gcc)

ans = struct with fields:

GainMargin: [1 1] PhaseMargin: [0 0] DiskMargin: 0 LowerBound: 0 UpperBound: 0 Frequency: NaN WorstPerturbation: [1x1 ss]

stepinfo(Gcc)

Warning: Simulation did not reach steady state. Please specify YFINAL if this system is stable and eventually settles.

ans = struct with fields:

RiseTime: NaN TransientTime: NaN SettlingTime: NaN SettlingMin: NaN SettlingMax: NaN Overshoot: NaN Undershoot: NaN Peak: Inf PeakTime: Inf

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

Whether the system is stable, minimum phase and proper:

isstable(Gcc)

ans = logical

0

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

ans = logical

0

isproper(Gcc)

ans = logical

1

Qc= ctrb(A,B)

Qc = 2×2

0 5.8860 5.8860 2.3544

controllab = rank(Qc)

controllab = 2

hasdelay(Gcc)

ans = logical

0

tdc3 = totaldelay(Gcc)

tdc3 = 0

sysndc3 = absorbDelay(Gcc)

sysndc3 = A = x1 x2 x3 x1 -2.425e+08 1.212e+06 -1.625e+05 x2 -4.853e+10 2.425e+08 -3.253e+07 x3 -1 0 0 B = u1 x1 0 x2 0 x3 1 C = x1 x2 x3 y1 1 0 0 D = u1 y1 0 Sample time: 0.01 seconds Discrete-time state-space model.

isallpass(tfdata(tf(Gcc),'v'))

ans = logical

0

% Stabilizing:

asys = ss(A,B,C,D)

asys = A = x1 x2 x1 0 1 x2 -0.05 0.4 B = u1 x1 0 x2 5.886 C = x1 x2 y1 1 0 D = u1 y1 0 Continuous-time state-space model.

opt2 = c2dOptions('Method','tustin','ThiranOrder',3,'DelayModeling','state');

dsys=c2d(asys,Ts,opt2)

dsys = A = x1 x2 x1 1 0.01002 x2 -0.000501 1.004 B = u1 x1 0.0002949 x2 0.05898 C = x1 x2 y1 1 0.00501 D = u1 y1 0.0001474 Sample time: 0.01 seconds Discrete-time state-space model.

nstates = 2

% observer model

%poles_obsv = exp(Ts*[-700 -310 -100 -90]);

poles_obsv = [-700 -310];

L=place(asys.a',asys.c',poles_obsv);

L=L';

Ah = asys.A;

bh = [asys.B L];

%cTh = eye(nstates);

%dh = [0 0;0 0;0 0;0 0];

%asysh=ss(Ah,bh,cTh,dh);

%figure

%step(asysh)

%add a state variable for the integral of the output error. This has the effect of adding an integral term to our controller which is known to reduce steady-state error.

%model for integral action

Ai = [[asys.A zeros(nstates,1)];-asys.C 0];

bi = [asys.B;0];

br = [zeros(nstates,1);1];

ci = [asys.C 0];

di = [0];

asysi=ss(Ai,bi,ci,di);

%Other augmentation scheme:

% Plant augmentation

Aaug=[asys.A zeros(nstates,1); zeros(1,nstates+1)];

Aaug(nstates+1,nstates)=1;

Baug=[asys.B;0];

Caug=eye(nstates+1);

Daug=zeros(nstates+1,1);

Plantcs=ss(Aaug,Baug,Caug,Daug);

% feedback controller

p1 = -800 + 800i;

p2 = -800 - 800i;

p3 = -400 - 400i;

%p1 = -110;

%p2 = -310;

%p3 = -500;

p4 = -400 + 400i;

p5 = -90;

%poles_k = exp(Ts*[p1 p2 p3 p4 p5]);

poles_k = [p1 p2 500];

K = place(asysi.a,asysi.b,poles_k)

K = 1×3

1.0e+08 * 0.0008 0.0000 1.0873

Ko = place(asys.A,asys.B,[p1 p2])

Ko = 1×2

1.0e+05 * 2.1747 0.0027

s4 = size(asys.A,1);

Z = [zeros([1,s4]) 1];

N = (1\([asys.A,asys.B;asys.C,asys.D]))*Z';

Nx = N(1:s4);

Nu = N(s4+1);

Nbar=Nu + Ko*Nx

Nbar = 1.6004e+03

%observer design:

At = [ asys.A-asys.B*Ko asys.B*Ko

zeros(size(asys.A)) asys.A-L*asys.C ];

Bt = [ asys.B*Nbar

zeros(size(asys.B)) ];

Ct = [ asys.C zeros(size(asys.C)) ];

%If you'd like to eliminate steady state error as much use Nbar:

% compute Nbar

%poles_k_d = exp(Ts.*poles_k)

%K_d = place(dsysi.a,dsysi.b,poles_k_d)

Ki=K(nstates+1);

K=K(1:nstates);

s4 = size(asys.A,1);

Z = [zeros([1,s4]) 1];

N = (1\([asys.A,asys.B;asys.C,asys.D]))*Z';

Nx = N(1:s4);

Nu = N(s4+1);

Nbarq=Nu + K*Nx

Nbarq = 1.1004e+03

%Well performing (adjusted) system:

syso = ss(At,Bt,Ct,0);

isstable(syso)

ans = logical

1

syso2 = minreal(syso)

2 states removed. syso2 = A = x1 x2 x1 0 1 x2 -1.28e+06 -1600 B = u1 x1 0 x2 9420 C = x1 x2 y1 1 0 D = u1 y1 0 Continuous-time state-space model.

%isminphase(syso)

f = 3;

t = 0:Ts:f;

figure

step(syso2,[0 f])

title('Continuous after treatment With observer flat(cold) start')

sysod = c2d(syso2,Ts,opt2)

sysod = A = x1 x2 x1 -0.561 0.0002439 x2 -312.2 -0.9512 B = u1 x1 0.01149 x2 2.298 C = x1 x2 y1 0.2195 0.000122 D = u1 y1 0.005744 Sample time: 0.01 seconds Discrete-time state-space model.

Ge = minreal(sysod)

Ge = A = x1 x2 x1 -0.561 0.0002439 x2 -312.2 -0.9512 B = u1 x1 0.01149 x2 2.298 C = x1 x2 y1 0.2195 0.000122 D = u1 y1 0.005744 Sample time: 0.01 seconds Discrete-time state-space model.

[b3, a3] = ss2tf(Ge.A,Ge.B,Ge.C,Ge.D)

b3 = 1×3

0.0057 0.0115 0.0057

a3 = 1×3

1.0000 1.5122 0.6098

Gest33 = tf(b3,a3,Ts) %discrete model

Gest33 = 0.005744 z^2 + 0.01149 z + 0.005744 ----------------------------------- z^2 + 1.512 z + 0.6098 Sample time: 0.01 seconds Discrete-time transfer function.

G = Ge

G = A = x1 x2 x1 -0.561 0.0002439 x2 -312.2 -0.9512 B = u1 x1 0.01149 x2 2.298 C = x1 x2 y1 0.2195 0.000122 D = u1 y1 0.005744 Sample time: 0.01 seconds Discrete-time state-space model.

DM = diskmargin(G)

DM = struct with fields:

GainMargin: [0.0015 656.0989] PhaseMargin: [-89.8253 89.8253] DiskMargin: 1.9939 LowerBound: 1.9939 UpperBound: 1.9939 Frequency: 291.5018 WorstPerturbation: [1x1 ss]

%[MS, WS] = sensitivity(G)

% L2 = ... your control system

figure

diskmarginplot(G)

title('After control')

figure

clf, nyquist(G), hold all;

diskmarginplot(DM.GainMargin,'nyquist')

hold off

%figure

%diskmarginplot(DM.DiskMargin,'disk')

isstable(sysod)

ans = logical

1

u = 0.001*ones(size(t));

x0 = [0.01 0];

figure

lsim(sysod,zeros(size(t)),t,[x0]);

title('Discrete sampled after treatment with observer and conditions hot start')

xlabel('Time (sec)')

ylabel('Output')

res_agent = sysod;

figure

w = logspace(2,6,1000);

bode(res_agent,w),grid

figure

rlocus(res_agent)

figure

nyquist(res_agent)

ms = minreal(res_agent);

Gcc = ms;

zms = zero(ms) % closed-loop zeros

zms =

-1.0000 + 0.0000i -1.0000 - 0.0000i

pms = pole(ms)

pms =

-0.7561 + 0.1951i -0.7561 - 0.1951i

tsim = 1;

setpointamp = 1; %

setpointapptime = 0.001; %

defaultsetpointpos = 0; %

Conf = RespConfig(Bias=defaultsetpointpos,Amplitude=setpointamp,Delay=setpointapptime)

Conf =

RespConfig with properties: Bias: 0 Amplitude: 1 Delay: 1.0000e-03 InitialState: [] InitialParameter: []

%figure

%step(Gcc,[0 tsim], Conf)

%title('Plant+Controller Closed Loop system step response')

[wngcc,zetagcc,pgcc] = damp(Gcc)

wngcc = 2×1

289.9608 289.9608

zetagcc = 2×1

0.0853 0.0853

pgcc =

-0.7561 + 0.1951i -0.7561 - 0.1951i

wngcc

wngcc = 2×1

289.9608 289.9608

zetagcc

zetagcc = 2×1

0.0853 0.0853

pgcc

pgcc =

-0.7561 + 0.1951i -0.7561 - 0.1951i

sigma(Gcc)

margin(Gcc)

diskmargin(Gcc)

ans = struct with fields:

GainMargin: [0.0015 656.0989] PhaseMargin: [-89.8253 89.8253] DiskMargin: 1.9939 LowerBound: 1.9939 UpperBound: 1.9939 Frequency: 291.5018 WorstPerturbation: [1x1 ss]

stepinfo(Gcc)

ans = struct with fields:

RiseTime: 0.0100 TransientTime: 0.1500 SettlingTime: 0.0500 SettlingMin: 0.0066 SettlingMax: 0.0085 Overshoot: 16.1214 Undershoot: 0 Peak: 0.0085 PeakTime: 0.0100

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

Whether the system is stable, minimum phase and proper:

isstable(sysod)

ans = logical

1

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

ans = logical

1

isproper(sysod)

ans = logical

1

Qc= ctrb(A,B)

Qc = 2×2

0 5.8860 5.8860 2.3544

controllab = rank(Qc)

controllab = 2

hasdelay(sysod)

ans = logical

0

%tdc3 = totaldelay(sysod)

%sysndc3 = absorbDelay(sysod)

isallpass(tfdata(tf(sysod),'v'))

ans = logical

0

%Less important:

%sys_cl = ss(Ai-bi*[K Ki],br,ci,di)

%[a,b] = ss2tf(sys_cl.A,sys_cl.B,sys_cl.C,sys_cl.D)

%[A,B,C,D] = tf2ss(Nbar*a,b)

%sys_cl = ss(A,B,C,D)

%figure

%step(sys_cl)

%discrete system

%hold on

%sys_cld = c2d(sys_cl,Ts)

%figure

%step(sys_cld)

%title('Discrete after treatment 2 sampled System')

0 Comments
Show -2 older commentsHide -2 older comments

Sign in to comment.

Full state feedback controller with I servomechanism

0 Comments
Show -2 older commentsHide -2 older comments

Answers (1)

0 Comments
Show -2 older commentsHide -2 older comments

See Also

Categories

Tags

Community Treasure Hunt

Full state feedback controller with I servomechanism

0 Comments Show -2 older commentsHide -2 older comments

Answers (1)

0 Comments Show -2 older commentsHide -2 older comments

See Also

Categories

Tags

Community Treasure Hunt

0 Comments
Show -2 older commentsHide -2 older comments

0 Comments
Show -2 older commentsHide -2 older comments