clc
clear
Ah=[-0.0177?6.4922?0?-9.8;
????-0.0012?-0.8739?1?0;
????0.0004?-4.4879?-0.9846?0;
????0?0?1?0];
Bh=[0?1.3032;
????-0.0561?-0.0002;
????-3.4193?-0.0144;
????0??0];
Ch=eye（44）;
Dh=zeros（42）;
pi1=52.5052;pi2=0.961;pi3=0;
pid1=0.0257;pid2=0.000062806;pid3=-0.00083172;
k=0.008;