tracker.m

倒立摆全套仿真程序

%无静差跟踪器数据计算
A=[0 1 0 0;51.58 0 0 0.5263;0 0 0 1;-7.737 0 0 -0.1789];
B=[0;-5.263;0;1.789];
C=[0 0 1 0];%选择小车位移作为输出
D=[0;0];

At=[A [0;0;0;0];-C 0];
Bt=[B;0];
rank(At);
Pt=[-2,-2,-3+2i,-3-2i,-5];%选择期望极点
M=ctrb(At,Bt);
J=diag(Pt);
phi=polyvalm(poly(J),At);
W=inv(M)*phi
Kt=[0 0 0 0 1]*inv(M)*phi;  
K=Kt(:,1:4)
Kc=-Kt(:,5)