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
4 次查看(过去 30 天)
显示 更早的评论
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.
0 个评论
回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Dynamic System Models 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!