Пример решения задачи стабилизирующего управления в пакете Octave .   Стабилизируется маятник с маховиком из механического практикума. Управление такое же, измерение - оба угла.

%http://wiki.octave.org/Control_package

M=0.04;
m=0.312;
Jm=0.0000803;
c1=0.0069;
c2=0.000099;
u0=19;
g=9.8;
l=0.3;
J=M*l^2/2;
sigma_mom=0.3;

a1=(M/2+m)/(J+m*l^2)*g*l;
a2=(J+m*l^2+Jm)/(J+m*l^2)/Jm;
c11=c1/(J+m*l^2);
c21=c2/(J+m*l^2);
c12=a2*c1;
c22=a2*c2;
A=[0 1 0 0; a1 0 0 c21; 0 0 0 1; -a1 0 0 -c22];
B=[0 0; 1/(J+m*l^2) -c11 ; 0 0;  0 -c12];
C=[0 0 0 1;1 0 0 0;0 0 1 0];
D=[0 0; 0 0; 0 0];

sys1=ss(A,B,C,D);
%{
[K,info1]=place(A,B(:,2),[ -1 -1.23 -5.7 -1.2]);
eig(A-B(:,2)*K)
[L,info1]=place(A',C(2:3,:)',[ -1 -1.23 -5.7 -1.01]);
eig(A-L'*C(2:3,:))
%}

controls=2;
sensors=[2:3];
%Q=diag([1/(2*3.14/360*7),1,1/u0]);
[K,S,e]=lqr(A,B(:,controls),eye(4),1);
[EST, L, P] = kalman (sys1, 1, eye(2),0,sensors, controls);

%est=estim(sys2,L')

%An=A-L'*C(2:3,:)-(B(:,2)-L'*D(2:3,2))*K;
%Cn=eye(length(A));
%est=ss(An,L',Cn,zeros(length(A),2))

%sysreg1=ss(est.a+B(:,2)*K,est.b,est.c(3:end,:)+D(3:end,2)*K,est.d(3:end,:));
%sysreg=K*est;
sysr=ss(EST.a-EST.b(:,1)*K,EST.b(:,2:3),-K*eye(4),zeros(1,2));

%syscl=feedback(sys1,sysreg,-1);
syscl=ss([A, B(:,controls)*sysr.c; sysr.b*C(2:3,:), sysr.a], [B(:,1); zeros(size(B(:,2)))]...
      , [C(1,:)   zeros(size(C(1,:)))], 0);

eig(sys1.a)
eig(syscl.a)
w=0.01:0.01:30*pi;
[Mag,PH]=bode(syscl(1,1),w);
%A11=squeeze(Mag(1,1,:));
figure(1)
plot(w,squeeze(Mag),'g')

figure(3)
sigma(sys1,syscl,w)

figure(5)
initial(sys1,[1 0 0 0])
figure(6)
initial(syscl,[1 0 0 0 0 0 0 0])

figure(7)
impulse(syscl)