guanqituoluojianmoyubianshichengxu.m

有关一种基于光纤角速率陀螺的建模与辨识程序

%光纤速率陀螺建模与辨识程序
G=1000;
G0=10000;
x=w(1:G);
x001=w(10000:10000+G0-1);
m=armax(x,[2,2],'SearchDirection','gn');
[C,P0,DE]=th2par(m);
a1=C(1,1);
a2=C(2,1);
d1=C(3,1);
d2=C(4,1);
b1=1;
b2=-1;
M=[b1.^2+b2.^2,1+a1.^2+a2.^2;b1*b2,a1+a1*a2;0,a2];
s=[(1+d1.^2+d2.^2)*DE;(d1+d1*d2)*DE;d2*DE];
S2=inv(M'*M)*M'*s;
Qx=S2(1,1);
Rx=S2(2,1);
I=eye(2);
A=[1,0;0,a2];
H=[1,1];
B=[0,1]';
Px=[100,0;100,0];
X=[x001(1),0]';
for t=2:G0
X=A*X;
x01(t)=X(1,1);
x02(t)=X(2,1);
P1x=A*Px*A.'+B*Qx*B.';
Kx=P1x*H.'*((H*P1x*H.'+Rx).^(-1));
Px=(I-Kx*H)*P1x;
X=X+Kx*(x001(t)-H*X);
x1(t)=X(1,1);
x2(t)=X(2,1);
p1x11(t)=P1x(1,1);
p1x12(t)=P1x(1,2);
p1x21(t)=P1x(2,1);
p1x22(t)=P1x(2,2);
px11(t)=Px(1,1);
px12(t)=Px(1,2);
px21(t)=Px(2,1);
px22(t)=Px(2,2);
end
 
X1=X;
t=G0-1;
s001(G0)=X(1,1);
s002(G0)=X(2,1);
while t>0
P1x=[p1x11(t+1),p1x12(t+1);p1x21(t+1),p1x22(t+1)];
Px=[px11(t),px12(t);px21(t),px22(t)];
Ax=Px*A'*inv(P1x);
X=[x1(t),x2(t)]';
X0=[x01(t+1),x02(t+1)]';
X1=X+Ax*(X1-X0);   
s001(t)=X1(1,1);
s002(t)=X1(2,1);
t=t-1;
end
for t=1:G0
s(t)=x001(t)-s001(t)-s002(t);
end