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

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

目录

一、测试程序

二、能控能观性判别函数

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

---

一、测试程序

A=[-2 0 0 0
    0 -5 1 0
    0 0 -5 1
    0 0 0 -5];
B=[2
    0
    0
    1];
C=[1 1 0 0];
[AA,BB,CC]=ss2obv(A,B,C);

二、能控能观性判别函数

function[ks,kv]=con_obv_Rank(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]=ss2obv(a,b,c)
[~,kg]=con_obv_Rank(a,b,c);
if kg==0
     error('the sys is unobservable')%如果系统不能观，则函数直接报错返回
 else
      disp('the sys is observable')
end
%% 求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=obsv(a,c);
iP=W*Qc;
%% 求A,B,C
C=c*inv(iP);
B=zeros(n,1);
B(n,:)=1;
A=iP*a*inv(iP);