I want to iterate my state space model, i am using lsim command right now. Can anybody help me with it? It will be of a great help

Question

Arjun Yeravdekar 2018-6-14

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/405757-i-want-to-iterate-my-state-space-model-i-am-using-lsim-command-right-now-can-anybody-help-me-with

编辑： Arjun Yeravdekar 2018-6-14

在 MATLAB Online 中打开

clc; clear all; close all;

%% reference path

W = 3;

endtime = 400;

Ts = 1;

L1 = 160;

L2 = 40;

L3 = 200;

L = L1 + L2 + L3;

T = 1:Ts:L;

psi = zeros(1,size(T,2));

x = zeros(1,size(T,2));

y = zeros(1,size(T,2));

for i = 1:size(T,2)

    if T(i) < L1
        y(i) = 0;
        x(i) = T(i);
        if i == 1
            psi(i) = atan((y(i)-0)/(x(i)-0));
        elseif i > 1
            psi(i) = atan((y(i)-y(i-1))/(x(i)-x(i-1)));
        end
    elseif T(i) >= L1 && T(i) <= (L1 + L2)
        x(i) = T(i);
        y(i) = (1/2)*W*cos((pi/L2)*x(i) - ((L1+L2)/L2)*pi) + (1/2)*W;
        psi(i) = atan((y(i)-y(i-1))/(x(i)-x(i-1)));
    elseif T(i) > (L2 + L1)
        y(i) = 3;
        x(i) = T(i);
        psi(i) = atan((y(i)-y(i-1))/(x(i)-x(i-1)));
    end

end

figure();

plot(y);

grid on;

figure();

plot(psi);

grid on;

%% Vehicle dynamics

m = 1200; %Kg

I = 1500; %Kgm^2

a = 0.92; %m

b = 1.38; %m

Cf = 120000; %N/rad

Cr = 80000; %N/rad

G = 17;

U = 30; %m/s

A = [-(Cf+Cr)/(U*m) (-(a*Cf-b*Cr)/(U*m))-U -0.79 0;

     -(a*Cf-b*Cr)/(U*I) -(a^2*Cf+b^2*Cr)/(U*I) 0 -0.000015;
     1 0 0 U;
     0 1 0 0];

B = [Cf/(m*G);

     (a*Cf)/(I*G);
     0;
     0];

C = [0 0 1 0;

0 0 0 1];

D = [0;

0];

H = ss(A,B,C,D); %generate a state space model

Hd = c2d(H,Ts); %converting the continuous time equation to discrete time with sampling time = 0.0001

% A,B,C,D after discretization

Ad = Hd.A; % A matrix after discretization

Bd = Hd.B; % B matrix after discretization

Cd = Hd.C; % C matrix after discretization

Dd = Hd.D; % D matrix after discretization

%% controller

theta = Cd*Bd;

Sq = eye(2);

% Sq(1,1) = 100;

M = ones(3,1);

M(1:2,1) = Sq*theta;

N = eye(3,2);

Kfull = M\N;

Kw = Kfull(1,:);

Cc = eye(4);

Dc = zeros(4,1);

w = Cd*Ad;

%% t = 0:Ts:endtime;

R = [y; psi];

e = ones(2,1);

% delta = zeros(1,size(T,2));

% V = zeros(4,size(T,2));

u = zeros(size(t,2),1);

e(1:2,1) = R(:,1);

xo = [0; 0; 0; 0];

for i = 1:size(T,2)-1

delta(i) = Kw*e(:,i);

% V(1:4,i) = Cc*(expm(Ad*t(i)))*Bd*delta(i);

    u(i,1) = delta(i);  
    [V] = lsim(Ad,Bd,Cc,Dc,u,t,xo);
    V_o = transpose(V);
    e(1:2,i+1) = R(:,i+1) - w*V_o(1:4,i);

end

figure();

plot(V_o(3,:))

grid on;

This is the code whic i have wirtten. I know lsim command takes an array of inputs. My input changes with the time. it is basically nothing but a closed loop system. I have a reference target which i am trying to follow . the error goes in the controller and the output of the controller goes inside the state space system which is nothing but my vehicle model. So as the times increases my reference also changes and accordingly my output will also change. But the problem which i am facing right now is that my error is increasing with time. i guess i am not using the lsim command properly. Can anybody help me out with it? I also tried iterating y(t) = C*expm(A*t)*B*u(t) but expm(A*t) is causing my output to blow up.