一般控制矩阵转能控标准型

新手码的代码，有很多内置函数都不清楚，欢迎大佬指路！

目录

一、测试程序

二、能控性判别函数

三、一般控制矩阵转能控标准型函数

---

一、测试程序

A=[0 1 3 4
    2 3 1 5
    3 4 5 1
    4 5 3 7];
C=[1 2 3 4];
B=[1;2;3;4];
[AA,BB,CC]=ss2con(A,B,C);

二、能控性判别函数

function[ks,kv]=controllability_RankCriterion(A,B,C)
%% 能控
n=size(A,1);
Qc=ctrb(A,B);
rankQc=rank(Qc);
if rankQc==n
    ks=1;
else
    ks=0;
end
%% 能观
Q0=obsv(A,C);
rankQc=rank(Q0);
if rankQc==n
    kv=1;
else
    kv=0;
end
end

三、一般控制矩阵转能控标准型函数

function[A,B,C]=ss2con(a,b,c)
[ks,~]=controllability_RankCriterion(a,b,c);
if ks==0
     error('the sys is uncontrollable')
else
     disp('the sys is controllable')
%% 求P矩阵
n=size(a,1);
A=zeros(n,n);
p=poly(a);
W=zeros(n,n);
w=zeros(1,n);
for i=1:n
    w(n-i+1)=p(i);
end
for i=1:n
    for j=1:n-i+1
        W(i,j)=w(i+j-1);
    end
end
Qc=ctrb(a,b);
P=Qc*W;
%% 求A,B,C
C=c*P;
B=zeros(n,1);
B(n,1)=1;
den=zeros(1,n);
for i=1:n
    den(i)=p(i+1);
end
for i=1:n-1
     A(i,i+1)=1;
end
dxn=fliplr(den);
dxn=-1*dxn;
A(n,:)=dxn;
end