www.pudn.com > Feedback-control-for-AUV.rar > parameter_heading_control_withObserver.m, change:2015-12-04,size:1607b


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=inv(M)*Cd; 
B=inv(M)*Dd; 
C=[0 1]; 
D=[0]; 
%Controllability & Observability 
rank_ctrb=rank(ctrb(A,B)); 
rank_obsv=rank(obsv(A,C)); 
% 
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)); 
 
%Feedback control without Observer 
%Respone plot 
t=0:0.01:10; 
u=10*ones(size(t)); 
x0=[0 1]; 
[y,t,x]=lsim(sys,u,t,x0); 
%plot(t,y); 
sys_cl=ss(A-B*K,B,C,D); 
%figure; 
[y,t,x]=lsim(sys_cl,u,t,x0); 
plot(t,y); 
figure; 
Nbar=rscale(sys,K); 
[y,t,x]=lsim(sys_cl,u*Nbar,t,x0); 
plot(t,y); 
 
%Observer design 
op1=-20; 
op2=-21; 
%L = (place(A',C',[op1 op2]))'; 
%L = ([0 0 1]*inv([C' A'*C' A'*A'*C'])*(A'^3+9*A^2+26*A+24*eye(3)))'; 
L = ([0 1]*inv([C' A'*C'])*(A'^2+41*A'+420*eye(2)))'; 
 
%This system state variables: [x1,x2,e1,e2]' 
At = [A-B*K B*K;zeros(size(A)) A-L*C]; 
Bt = [B*Nbar;zeros(size(B))]; 
Ct = [C zeros(size(C))]; 
 
sys1 = ss(At,Bt,Ct,D); 
e0=x0; 
%Respone plot 
[y,t,x] = lsim(sys1,u,t,[x0 e0]); 
% 
figure; 
plot(t,y); 
% 
e = x(:,3:end); 
x = x(:,1:2); 
x_est = x-e; 
 
% Save state variables explicitly to aid in plotting 
x1 = x(:,1); x2 = x(:,2); 
x1_est = x_est(:,1); x2_est = x_est(:,2); 
 
figure; 
plot(t,x1,'-r',t,x1_est,':r'); 
figure; 
plot(t,x2,'-b',t,x2_est,':b'); 
figure; 
plot(t,x1,'-r',t,x1_est,':r',t,x2,'-b',t,x2_est,':b'); 
xlabel('Time (sec)');