www.pudn.com > Feedback-control-for-AUV.rar > parameter_heading_control.m, change:2016-05-13,size:813b


clear all; 
SimulationTime=10;%second 
StepTime=0.01;%second 
Izz=3.45;%kgm^2 
Nr_dot=-4.88;%kgm^2 
Nr=-7;%kgm/s 
Ndelta_r=-54.54;%kgm/s^2 
U=2;%m/s 
%Kp=10; 
td=0.01; 
M=[Izz-Nr_dot 0;0 1]; 
Cd=[Nr 0;1 0]; 
Dd=[Ndelta_r;0]; 
A=M\Cd; 
B=M\Dd; 
C=[0 1]; 
D=[0]; 
%Controllability & Observability 
rank_ctrb=rank(ctrb(A,B)); 
rank_obsv=rank(obsv(A,C)); 
%% 
%Open loop 
sys=ss(A,B,C,D); 
sysl=tf(sys); 
op1 = -1.15+1.57i; 
op2 = -1.15-1.57i; 
%K = place(A,B,[op1 op2]); 
K=[0 1]*inv([B A*B])*(A^2+2.3*A+3.7874*eye(2)); 
t=0:0.01:10; 
u=10*ones(size(t)); 
x0=[0 0]; 
[y,t,x]=lsim(sys,u,t,x0); 
% 
figure; 
plot(t,y); 
 
%% 
%Close loop 
sys_cl=ss(A-B*K,B,C,D); 
% 
figure; 
[y,t,x]=lsim(sys_cl,u,t,x0); 
plot(t,y); 
%% 
%Compensate gain 
Nbar=rscale(sys,K); 
% 
figure; 
[y,t,x]=lsim(sys_cl,u*Nbar,t,x0); 
plot(t,y);