查看: 233|回复: 0|关注: 0

[未答复] 谁能帮我看看以下可控性问题

[复制链接]

新手

5 麦片

财富积分


050


1

主题

1

帖子

0

最佳答案
发表于 2020-1-17 21:41:07 | 显示全部楼层 |阅读模式
我做的是多智能体的可控性,根据Gram判别法,在我设计的控制输入下,任意给定的初值,系统的轨迹都会最终到达我给定的终端X1点。

具体到我考虑的例子中,我给定的终端X1是在同一直线上,但是我编写的程序无法达到预期。即最终没有走到终端X1这点。我估计我的程序出问题了,但我找不到问题在哪里。
表现异常的地方有:一个是轨迹在时间终点没有达到我给定的终端,另一个是积分计算的值太大,可能有误,再一个是Gram矩阵(对应程序Transform)计算结果也太大。

代码如下:

function StateOfSystem()
%
%---<注释符号>---%:仿真多智能体
%
clc;
clear;
tau=2;
t1=8;
%---<注释符号>---%:这里终端时间要求要满足 t1>=(Nn-1)tau;
h1=0.1;
N=t1/h1;
t=0:h1:t1;
x11=[15;-5;25];
x12=[17;-6;28];
x13=[19;-7;31];
x14=[21;-8;34];
X1=[x11;x12;x13;x14];
n=10;
h=tau/n;

td=-tau:h:0;
tf=[td,tau:tau:t1];

X=zeros(12,N+1);
W=zeros(12,n+1);
for i=1:n+1     
    W(:,i)=InitialValue(td(i));     
end  
X(:,1)=HomoSolution(tau,t(1));
for j=2:N+1

%--------------------<分隔符>--------------------%
%---<注释符号>---%:这里可以写一个判断函数,确定两个矩阵在一定容许误差下相等;再用单位矩阵替换。
Unit=Nonhomogeneous(t1,tau,t(j))/(Transform(t1,tau));
Det=det(Unit);
I=eye(n);
for r=1:n
    for s=1:n
        m1=abs(Det-1);
        m2= abs(Unit(r,s)-I(r,s));
        if t(j)==t1 && m1<10^-10 && m2<10^-10
            warning('Should be Regarded as Unit Matrix');
            Unit=subs(eye(n));
        end
    end
end
%--------------------<分隔符>--------------------%
    X(:,j)=HomoSolution(tau,t(j))-Unit*(HomoSolution(tau,t(N+1))-X1);
    %---<注释符号>---%:有必要测试一下程序是否能执行括号运算;估计新版的matlab对括号运算逻辑存在问题。
    test1=Nonhomogeneous(t1,tau,t(j))
    test2=Transform(t1,tau)
    test3=Nonhomogeneous(t1,tau,t(j))/Transform(t1,tau)
end  
x1=X(:,N+1)
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
W11=zeros(1,n+1);
W12=zeros(1,n+1);
W13=zeros(1,n+1);

W21=zeros(1,n+1);
W22=zeros(1,n+1);
W23=zeros(1,n+1);

W31=zeros(1,n+1);
W32=zeros(1,n+1);
W33=zeros(1,n+1);

W41=zeros(1,n+1);
W42=zeros(1,n+1);
W43=zeros(1,n+1);

for r=1:n+1
    W11(:,r)=W(1,r);
    W12(:,r)=W(2,r);
    W13(:,r)=W(3,r);

    W21(:,r)=W(4,r);
    W22(:,r)=W(5,r);
    W23(:,r)=W(6,r);

    W31(:,r)=W(7,r);
    W32(:,r)=W(8,r);
    W33(:,r)=W(9,r);

    W41(:,r)=W(10,r);
    W42(:,r)=W(11,r);
    W43(:,r)=W(12,r);
end
X11=zeros(1,N+1);
X12=zeros(1,N+1);
X13=zeros(1,N+1);

X21=zeros(1,N+1);
X22=zeros(1,N+1);
X23=zeros(1,N+1);

X31=zeros(1,N+1);
X32=zeros(1,N+1);
X33=zeros(1,N+1);

X41=zeros(1,N+1);
X42=zeros(1,N+1);
X43=zeros(1,N+1);
for k=1:N+1
    X11(:,k)=X(1,k);
    X12(:,k)=X(2,k);
    X13(:,k)=X(3,k);

    X21(:,k)=X(4,k);
    X22(:,k)=X(5,k);
    X23(:,k)=X(6,k);

    X31(:,k)=X(7,k);
    X32(:,k)=X(8,k);
    X33(:,k)=X(9,k);

    X41(:,k)=X(10,k);
    X42(:,k)=X(11,k);
    X43(:,k)=X(12,k);
end
x1=2*tf+1;
y1=-tf+2;
z1=3*tf+4;
%---<注释符号>---%:正确的图像,X11,X12,X13的值应该等于 x11=[2;24;5];
%---<注释符号>---%:以下是绘图程序;
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
Y11=X11(1,N+1);
Y12=X12(1,N+1);
Y13=X13(1,N+1);

Y21=X21(1,N+1);
Y22=X22(1,N+1);
Y23=X23(1,N+1);

Y31=X31(1,N+1);
Y32=X32(1,N+1);
Y33=X33(1,N+1);

Y41=X41(1,N+1);
Y42=X42(1,N+1);
Y43=X43(1,N+1);
%--------------------<分隔符>--------------------%
Z11=X11(1,1);
Z12=X12(1,1);
Z13=X13(1,1);

Z21=X21(1,1);
Z22=X22(1,1);
Z23=X23(1,1);

Z31=X31(1,1);
Z32=X32(1,1);
Z33=X33(1,1);

Z41=X41(1,1);
Z42=X42(1,1);
Z43=X43(1,1);
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
%以下是单独画每组状态变量自己的曲线走势图;
%状态x1,x2,x3的图;
figure;
plot(td,W11,'r-.',t,X11,'r-.',t1,Y11,'r*');  
xlabel('$x$','Interpreter','latex','FontSize',18);
ylabel('$$y$$','Interpreter','latex','FontSize',18);
set(gca,'FontSize',10);
grid on;
hold on
plot(td,W12,'b-.',t,X12,'b-.',t1,Y12,'b*');  
plot(td,W13,'k-.',t,X13,'k-.',t1,Y13,'k*');  
hold off
%--------------------<分隔符>--------------------%
%状态x4,x5,x6的图;
figure;
plot(td,W21,'r-.',t,X21,'r-.',t1,Y21,'r*');  
xlabel('$x$','Interpreter','latex','FontSize',18);
ylabel('$$y$$','Interpreter','latex','FontSize',18);
set(gca,'FontSize',10);
grid on;
hold on
plot(td,W22,'b-.',t,X22,'b-.',t1,Y22,'b*');  
plot(td,W23,'k-.',t,X23,'k-.',t1,Y23,'k*');  
hold off
%--------------------<分隔符>--------------------%
%状态x7,x8,x9的图;
figure;
plot(td,W31,'r-.',t,X31,'r-.',t1,Y31,'r*');  
xlabel('$x11$','Interpreter','latex','FontSize',18);
ylabel('$$x12$$','Interpreter','latex','FontSize',18);
set(gca,'FontSize',10);
grid on;
hold on
plot(td,W32,'b-.',t,X32,'b-.',t1,Y32,'b*');  
plot(td,W33,'k-.',t,X33,'k-.',t1,Y33,'r*');  
hold off
%--------------------<分隔符>--------------------%
%状态x10,x11,x12的图;
figure;
plot(td,W41,'r-.',t,X41,'r-.',t1,Y41,'r*');  
xlabel('$x11$','Interpreter','latex','FontSize',18);
ylabel('$$x12$$','Interpreter','latex','FontSize',18);
set(gca,'FontSize',10);
grid on;
hold on
plot(td,W42,'b-.',t,X42,'b-.',t1,Y42,'b*');  
plot(td,W43,'k-.',t,X43,'k-.',t1,Y43,'r*');  
hold off
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
%下面绘制系统状态曲线图像;每条曲线由三维组成,总共四个状态。
figure;
plot3(x1,y1,z1,'y-');  
xlabel('$x$','Interpreter','latex','FontSize',18);
ylabel('$$y$$','Interpreter','latex','FontSize',18);
zlabel('$$z$$','Interpreter','latex','FontSize',18);
set(gca,'FontSize',10);
grid on;
%--------------------<分隔符>--------------------%
hold on;
plot3(W11,W12,W13,'b-',Z11,Z12,Z13,X11,X12,X13,'b-',Y11,Y12,Y13,'b*');  
plot3(W21,W22,W23,'g-',Z21,Z22,Z23,X21,X22,X23,'g-',Y21,Y22,Y23,'g*');  
plot3(W31,W32,W33,'r-',Z31,Z32,Z33,X31,X32,X33,'r-',Y31,Y32,Y33,'r*');  
plot3(W41,W42,W43,'k-',Z41,Z42,Z43,X41,X42,X43,'k-',Y41,Y42,Y43,'k*');  
hold off;
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%

function T_funHomo=HomoSolution(tau,t)
%      
%---<注释符号>---%:齐次方程对应的解,eta的值也是通过这个函数计算;
%--------------------<分隔符>--------------------%
%--------------------<分隔符>--------------------%
% A=[2,-6,-2;3,-7,-6;-3,6,7];
% B=[5,-6,-4;1,0,-2;0,0,3];
% CK =[15,-36,-26;4,-11,-8;3,-6,-4];
% CP =[14,-30,-22;3,-7,-6;3,-6,-3];
[A,B,C,CK,CP]=ParaCptAnsWay();
D=[1,0,0,0;0,6,0,0;0,0,3,0;0,0,0,9];
L=[13,-6,-7,0;-5,10,-2,-3;-4,-4,13,-5;0,-8,-2,10];
F1=[1,0;0,6;3,0;0,9];
F=kron(F1,CP);
I4=eye(4);
Bhat=kron(I4,expm(-A*tau))*(kron(I4,B)-kron(L,CK)-kron(D,CP));
%--------------------<分隔符>--------------------%

N=1000;
fun=@(x)kron(I4,expm(A*(x+tau)))*DelayMatrix(Bhat,tau,x)*(InitialValueDeriva(t-x-tau)-kron(I4,A)*InitialValue(t-x-tau));
T_fun1=double(Simpson(fun,N,t-tau,t));
T_fun2=kron(I4,expm(A*(t+tau)))*DelayMatrix(Bhat,tau,t)*InitialValue(-tau);
%I=[1;1;1;1;1;1;1;1;1;1;1;1];
%T_fun2=kron(I4,expm(eye(3)*(t+tau)))*I;
%T_fun2=DelayMatrix(Bhat,tau,t)*I;
T_funHomo=T_fun1+T_fun2;

end
%--------------------<分隔符>--------------------%


function T_funNonho=Nonhomogeneous(t1,tau,t)
%        
%---<注释符号>---%:求解积分项; 这里的t0,有时是作为积分的下限,有时是作为指数函数的初值,因此在调用函数时,要特别小心;
%--------------------<分隔符>--------------------%
%
x11=[15;-5;25];
x12=[17;-6;28];
x13=[19;-7;31];
x14=[21;-8;34];
X1=[x11;x12;x13;x14];
%---<注释符号>---%:x1相当于终端值;

%--------------------<分隔符>--------------------%
[A,B,C,CK,CP]=ParaCptAnsWay();
D=[1,0,0,0;0,6,0,0;0,0,3,0;0,0,0,9];
L=[13,-6,-7,0;-5,10,-2,-3;-4,-4,13,-5;0,-8,-2,10];
F1=[1,0;0,6;3,0;0,9];
F=kron(F1,CP);

I4=eye(4);
Bhat=kron(I4,expm(-A*tau))*(kron(I4,B)-kron(L,CK)-kron(D,CP));
DCP=F*F';
%---<注释符号>---%:这个k是通过t/tau计算出来的,随后要做出更改;

k=t/tau;
n=size(Bhat);

N=1000;
Gram=zeros(n);

% for i=0:k-1
%     Gram3=@(x)kron(I4,expm(A*x))*DelayMatrix(Bhat,tau,x)*DCP*DelayMatrix(Bhat',tau,x)*kron(I4,expm(A'*x));
%     %Gram3=@(x)kron(I4,expm(A*x))*DelayMatrix(Bhat,tau,x)*DCP*DelayMatrix(Bhat',tau,x+t1-t)*kron(I4,expm(A'*(x+t1-t)));   
%     Gram4=SimpsonSqrMat(Gram3,N,(i-1)*tau,i*tau);
%     Gram=Gram+Gram4;
% end


Gram3=@(x)kron(I4,expm(A*(t-x-tau)))*DelayMatrix(Bhat,tau,(t-x-tau))*DCP*DelayMatrix(Bhat',tau,(t-x-tau))*kron(I4,expm(A'*(t-x-tau)));
Gram=SimpsonSqrMat(Gram3,N,0,t);


% Unit=Gram/(Transform(t1,tau));
% %--------------------<分隔符>--------------------%
% %---<注释符号>---%:这里可以写一个判断函数,确定两个矩阵在一定容许误差下相等;再用单位矩阵替换。
% Det=det(Unit);
% I=eye(n);
% for r=1:n
%     for s=1:n
%         m1=abs(Det-1);
%         m2= abs(Unit(r,s)-I(r,s));
%         if t==t1 && m1<10^-10 && m2<10^-10
%             warning('Should be Regarded as Unit Matrix');
%             Unit=subs(eye(n));
%         end
%     end
% end
%--------------------<分隔符>--------------------%

T_funNonho=Gram;

return;

function T_funGram=Transform(t1,tau)
%         
%---<注释符号>---%:计算连续时的状态转移矩阵。
%---<注释符号>---%:这里终端时间要求要满足 t1>=(Nn-1)tau;
K=t1/tau;
%--------------------<分隔符>--------------------%
% A=[2,-6,-2;3,-7,-6;-3,6,7];
% B=[5,-6,-4;1,0,-2;0,0,3];
% CK =[15,-36,-26;4,-11,-8;3,-6,-4];
% CP =[14,-30,-22;3,-7,-6;3,-6,-3];

[A,B,C,CK,CP]=ParaCptAnsWay();
D=[1,0,0,0;0,6,0,0;0,0,3,0;0,0,0,9];
L=[13,-6,-7,0;-5,10,-2,-3;-4,-4,13,-5;0,-8,-2,10];
F1=[1,0;0,6;3,0;0,9];
F=kron(F1,CP);

I4=eye(4);
Bhat=kron(I4,expm(-A*tau))*(kron(I4,B)-kron(L,CK)-kron(D,CP));
%--------------------<分隔符>--------------------%
DCP=F*F';
N=1000;
n=size(Bhat);
Gram=zeros(n);
% GramIntg=@(x)kron(I4,expm(A*x))*DelayMatrix(Bhat,tau,x)*DCP*DelayMatrix(Bhat',tau,x)*(kron(I4,expm(A'*x)));
% for j=0:K-1     
%     Gram=Gram+SimpsonSqrMat(GramIntg,N,(j-1)*tau,j*tau);
% end
%--------------------<分隔符>--------------------%(t1-x-tau)
GramIntg=@(x)kron(I4,expm(A*(t1-x-tau)))*DelayMatrix(Bhat,tau,(t1-x-tau))*DCP*DelayMatrix(Bhat',tau,(t1-x-tau))*(kron(I4,expm(A'*(t1-x-tau))));
Gram=SimpsonSqrMat(GramIntg,N,0,t1);
T_funGram=double(Gram);

return;

function y=InitialValue(t)
%
%---<注释符号>---%:给定初值;

y1=(100*sin(0.05*t));
y2=(-100*cos(0.05*t));
y3=(-30*exp(-5*t));

y4=150*cos(0.05*t);
y5=-130*sin(0.05*t);
y6=-300*cos(0.05*t);

y7=100*t^2;
y8=-10*exp(t);
y9=30*t^2;

y10=-120*t^3;
y11=10*exp(-t);
y12=200*cos(0.05*t);
%--------------------<分隔符>--------------------%
y=[y1;y2;y3;y4;y5;y6;y7;y8;y9;y10;y11;y12];

return;



function y=InitialValueDeriva(t)
%
%---<注释符号>---%:给定初值;

y1=(5*cos(0.05*t));
y2=(5*sin(0.05*t));
y3=(150*exp(-5*t));

y4=-7.5*sin(0.05*t);
y5=-6.5*cos(0.05*t);
y6=15*sin(0.05*t);

y7=200*t;
y8=-10*exp(t);
y9=60*t;

y10=-360*t^2;
y11=-10*exp(-t);
y12=-10*sin(0.05*t);

%--------------------<分隔符>--------------------%
y=[y1;y2;y3;y4;y5;y6;y7;y8;y9;y10;y11;y12];


% J1=[1;1;1;1;1;1;1;1;1;1;1;1];
% y=cos(t).*J1;

return;


function Dely=DelayMatrix(Bhat,tau,t)
%        
%---<注释符号>---%:延迟矩阵;
%---<注释符号>---%:这种写法,可能只使用于t=k*tau的情形;因此,对t取其他的其他的要做更改。
%---<注释符号>---%:(k-1)*tau<=t<k*tau;两边同时除以tau,得:(k-1)<=t/tau<k.
[a]=size(Bhat);
I=Bhat^0;
Mat=zeros(a);
td=t/tau;
K=ceil(td);
if t==-1
    Mat=I;
else   
    for k=0:K        
        Mat=Mat+Bhat^k*(t-(k-1)*tau)^k/factorial(k);
    end
end
Dely=Mat;

return;


function g=Simpson(f,N,a,b)
%      
%--------------------<分隔符>--------------------%
%
%format long
%--------------------<分隔符>--------------------%
n=N/2;
h=(b-a)/N;
x=a:h:b;
fv=zeros(12,1,N+1);
%fv=zeros(1,1,N+1);
for i=1:N+1
    fv(:,:,i)=f(x(i));
end
s1=zeros(12,1);
%s1=zeros(1,1);
for j=1:2:2*n-1
    s1=s1+fv(:,:,j);
end
w1=s1*4;
s2=zeros(12,1);
%s2=zeros(1,1);
for j=2:2:2*n
    s2=s2+fv(:,:,j);
end
w2=s2*2;
w3=w1+w2+f(a)+f(b);   

w4=w3*h/3;
g=w4;


return;


function g=SimpsonSqrMat(f,N,a,b)
%          SimpsonSqrMat(GramIntg,N,(j-1)*tau,j*tau)
%--------------------<分隔符>--------------------%
%
%format long
%--------------------<分隔符>--------------------%
n=N/2;
h=(b-a)/N;
x=a:h:b;
fv=zeros(12,12,N+1);
for i=1:N+1
    fv(:,:,i)=f(x(i));
end
s1=zeros(12,12);
for j=1:2:2*n-1
    s1=s1+fv(:,:,j);
end
w1=s1*4;
s2=zeros(12,12);
for j=2:2:2*n
    s2=s2+fv(:,:,j);
end
w2=s2*2;
w3=w1+w2+f(a)+f(b);   

w4=w3*h/3;
g=w4;

return;


function [A,B,C,CK,CP]=ParaCptAnsWay()
tau=1;
A=[2,-6,-2;
    3,-7,-6;
    -3,6,7];
M=[2,2,3;
    0,1,-1;
    1,0,3];
invM=inv(M);
DeltA1=invM*A*M;

DeltA=[1,0,0;0,-1,0;0,0,2];
A1=M*DeltA*invM;

DeltB=[23,0,0;0,12,0;0,0,30];
B=M*DeltB*invM;
AB=A*B;
BA=B*A;

DeltK=[12,0,0;0,-30,0;0,0,10];
CK=M*DeltK*invM;
ACK=A*CK;
CKA=CK*A;
K=inv(M);
C=CK*(M)*2;

ACK=A*C*K;
CKA=C*K*A;

DeltP=[30,0,0;0,-10,0;0,0,20];
CP=M*DeltP*invM;
ACP=A*CP;
CPA=CP*A;


D=[1,0,0,0;0,6,0,0;0,0,3,0;0,0,0,9];
L=[13,-6,-7,0;-5,10,-2,-3;-4,-4,13,-5;0,-8,-2,10];
F1=[1,0;0,6;3,0;0,9];
F=kron(F1,CP);
DCP=F*F';
I4=eye(4);
Bhat=kron(I4,expm(-A*tau))*(kron(I4,B)-kron(L,CK)-kron(D,CP));
%--------------------<分隔符>--------------------%

DeltE1=expm(-DeltA);
DeltE=[exp(-1),0,0;0,exp(1),0;0,0,exp(-2)];
BhatN=kron(I4,M)*kron(I4,DeltE)*(kron(I4,DeltB)-kron(L,DeltK)-kron(D,DeltP))*kron(I4,invM);

return;
























您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

站长推荐上一条 /4 下一条

快速回复 返回顶部 返回列表