%%%%%%%%%%%%%%%%%%%%数学模型%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = [0,7,0,0;0,0,0,7/3;0,0,0,0;0,0,0,0];
B = [0,0;0,0;1,0;0,1];
C = [1,0,0,0;0,0,1,0;0,0,0,1];
D = 0;
x0_poleconfig=[5,pi/2,7,0];
x0_stateobserver=[5,pi/2,7,0,5,pi/2,7,0,5,pi/2,7,0];
G = ss(A,B,C,D);
G1 = tf(G);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%稳定性%%%%%%%%%%%%%%%%%%%%
key = isstable(G); % 是否稳定 1 稳定 0 不稳定
%用特征根来判断
[VV,DD]=eig(A);
V=diag(DD);
re=real(V);
if(~isempty(find(re>=0, 1)))
disp('The system is unstable!')
re(re>0)
else
disp('The system is stable!');
end
%用极点是否具有负实部判定是否稳定
P=poly(A);
v=roots(P);
Re=real(v);
if(~isempty(find(Re>=0, 1)))
disp('The system is unstable!')
v(Re>0)
else
disp('The system is stable!');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%能控性%%%%%%%%%%%%%%%%%%%%
SizeofA=size(A);
%能控性第一个判据：能控性判定矩阵
Tc = ctrb(A,B); %能控性判定矩阵
rTc = rank(Tc); %满秩可控，反之不可控
%显示能控性判定矩阵的秩
disp('能控性判定矩阵的秩:');rTc
disp('A的维数:');SizeofA
if (rTc == SizeofA)
disp('The system is controllable')
else
disp('The system is uncontrollable')
end
% %约旦标准型判据%矩阵B不含零行向量
% %求约旦标准型
% [Gctrljordan,Tctrl] =canon(G,'modal');
% disp('能控性约旦标准型判据:');Gctrljordan
% %求能控标准型
% %Gctrlnorm =sscanform(G,'ctrl');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%能观性%%%%%%%%%%%%%%%%%%%%
%与能控是对偶关系
To = obsv(A,C); %能观性判定矩阵
rTo = rank(To); %满秩能观，反之不能观
disp('能观性判定矩阵的秩:');rTo
disp('A的维数:');SizeofA
if (rTo == SizeofA)
disp('The system is observable')
else
disp('The system is unobservable')
end
% %约旦标准型判据%矩阵C不含零列向量
% %求约旦标准型
% [Gobsvjordan,Tobsv] =canon(G,'modal');
% disp('能观性约旦标准型判据:');Gobsvjordan
% %求能观标准型
% %Gobsvnorm =sscanform(G,'obsv');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%极点配置控制器设计%%%%%%%%%%%%%%%%%%%%
%通过向量配置闭环系统极点,P为期望极点 %稳定性
P =[-10+2j,-10-2j,-2,-2];
K=place(A,B,P);%状态反馈矩阵%
Gpoleconfig=ss(A-B*K,B,C,D);
[y1_poleconfig,t1_poleconfig,x1_poleconfig]=initial(Gpoleconfig,x0_poleconfig,10);
[y2_poleconfig,t2_poleconfig,x2_poleconfig]=step(Gpoleconfig,10);
y_poleconfig = y1_poleconfig+ y2_poleconfig;
t_poleconfig = t1_poleconfig;
x_poleconfig = x1_poleconfig + x2_poleconfig;
%求约旦标准型
[Gobsvjordan,Tobsv] =canon(Gpoleconfig,'modal');
disp('能观性约旦标准型判据:');Gobsvjordan
%求能观标准型
%Gobsvnorm =sscanform(G,'obsv');
% %约旦标准型判据%矩阵B不含零行向量
% %求约旦标准型
[Gctrljordan,Tctrl] =canon(Gpoleconfig,'modal');
disp('能控性约旦标准型判据:');Gctrljordan
% %求能控标准型
% %Gctrlnorm =sscanform(G,'ctrl');
%极点配置控制器阶跃响应
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%极点配置控制器的阶跃响应
figure(1);
plot(x_poleconfig(:,:,1));
grid on;
legend('后轮中心点M距X轴距离','车体航向角',"两前轮中心点的速度",'前轮转向角');
xlabel('t');
ylabel('状态变量阶跃响应');
title('极点配置状态变量阶跃响应（输入1）');
figure(2);
plot(x_poleconfig(:,:,2));
grid on;
legend('后轮中心点M距X轴距离','车体航向角',"两前轮中心点的速度",'前轮转向角');
xlabel('t');
ylabel('状态变量阶跃响应');
title('极点配置状态变量阶跃响应（输入2）');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%基于观测器的调节器设计%%%%%%%%%%%%%%%%%%%%
L=(place(A',C',P))';%L-观测器向量 P-期望极点向量
[Gc,H]=obsvsf(G,K,L);
Gstateobserver=feedback(G*Gc,H);
[y1_stateobserver,t1_stateobserver,x1_stateobserver]=initial(Gstateobserver,x0_stateobserver,10);
[y2_stateobserver,t2_stateobserver,x2_stateobserver]=step(Gstateobserver,10);
y_stateobserver= y1_stateobserver+ y2_stateobserver;
t_stateobserver= t1_stateobserver;
x_stateobserver= x1_stateobserver + x2_stateobserver;%基于观测器控制器的阶跃响应
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%基于观测器的阶跃响应
figure(3);
plot(x_stateobserver(:,:,1));
grid on;
legend('后轮中心点M距X轴距离','车体航向角',"两前轮中心点的速度",'前轮转向角');
xlabel('t');
ylabel('状态变量阶跃响应');
title('观测器状态变量阶跃响应（输入1）');
figure(4);
plot(x_stateobserver(:,:,2));
grid on;
legend('后轮中心点M距X轴距离','车体航向角',"两前轮中心点的速度",'前轮转向角');
xlabel('t');
ylabel('状态变量阶跃响应');
title('观测器状态变量阶跃响应（输入2）');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%极点配置和状态观测器两者的比较%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%比较两者的阶跃响应
figure(5);
plot(t_poleconfig,y2_poleconfig(:,2,1),'r',t_stateobserver,y2_stateobserver(:,2,1),'b');
grid on;
legend('ypoleconfig','ystateobserver');
xlabel('t');
ylabel('状态变量阶跃响应');
title('状态变量阶跃响应');
%比较两者的最小实现
Gpoleconfigminreal=zpk(minreal(Gpoleconfig));
Gstateobserverminreal=zpk(minreal(Gstateobserver));
%显示两个控制器的最小实现
Gpoleconfigminreal
Gstateobserverminreal
function [Gc,H]=obsvsf(G,K,L)
H=ss(G.a-L*G.c,L,K,0);
Gc=ss(G.a-G.b*K-L*G.c+L*G.d*K,G.b,-K,eye(2));
end
