dhfz_jmbqd.asv

希望每一个使用本程序的人都能做到保密

clc
clear
format short e
%******************************************************************
g=9.78;
angle=45*pi/180;
wie=7.2921158*10^(-5);
Rad_Degree=1/180.0*pi; % “度”转为“弧度”
zh=1/180*pi/3600;% “度/小时”转换成“弧度/秒”
a=zeros(10);
%*********************************
a(1,2)=2*wie*sin(angle);
a(1,4)=g;
a(1,6)=1;
a(2,1)=-a(1,2);
a(2,3)=-g;
a(2,7)=1;
a(3,4)=wie*sin(angle);
a(3,5)=-wie*cos(angle);
a(3,8)=1;
a(4,3)=-wie*sin(angle);
a(4,9)=1;
a(5,3)=wie*cos(angle);
a(5,10)=1;
%****************************************
%***************************************
c=zeros(5,7);
c(1,1)=1;
c(2,2)=1;
c(3,4)=g;
c(4,3)=-g;
c(5,3)=-3*g*wie*sin(angle);
c(5,5)=g*wie*cos(angle);
%*****************************************
aa=[a(1,:);a(2,:);a(3,:);a(4,:);a(5,:);a(9,:);a(10,:)];
A=[aa(:,1) aa(:,2) aa(:,3) aa(:,4) aa(:,5) aa(:,9) aa(:,10)];
%*****************************************
B=[eye(7) zeros(7,2)];
%*********************************************
%C=c;
C=zeros(2,7)
C(1,1)=1;
C(2,2)=1;
%********************************************
gama=3;
L=zeros(2,7);
L(1,3)=0.01;
L(2,4)=0.01;
%******************************************************************
deta_A=zeros(7);
deta_A(1,2)=-1;
deta_A(2,1)=1;
deta_A(3,3)=1;
deta_A(4,3)=-1;
A1=A+deta_A;
%******************************************************************
deta_A=zeros(7);
deta_A(1,2)=01;
deta_A(2,1)=-0.1;
deta_A(3,3)=-0.1;
deta_A(4,3)=0.1;
A2=A+deta_A;
%******************************************************************
D=[zeros(2,7) eye(2)];
%*******************************************************************
%*******************************************************************
setlmis([])
X=lmivar(1,[7 1])
R=lmivar(1,[7 1])
M=lmivar(2,[7 7])
Z=lmivar(2,[7 2])
%******************************************************************
%******************************************************************
lmiterm([1 1 1 R],1,A1,'s')
lmiterm([1 2 1 R],A1',1)
lmiterm([1 2 1 X],1,A1)
lmiterm([1 2 1 Z],1,C)
lmiterm([1 2 1 M],1,1)
lmiterm([1 1 3 R],1,B)
lmiterm([1 4 1 0],L)
lmiterm([1 4 1 0],0)
lmiterm([1 4 1 0],-L)
lmiterm([1 2 2 X],1,A1,'s')
lmiterm([1 2 2 Z],1,C,'s')
lmiterm([1 2 3 X],1,B)
lmiterm([1 2 3 Z],1,D)
lmiterm([1 4 2 0],L)
lmiterm([1 4 2 0],0)
lmiterm([1 3 3 0],-1)
lmiterm([1 4 3 0],0)
lmiterm([1 4 4 0],-gama^2)
%******************************************************************
lmiterm([2 1 1 R],1,A2,'s')
lmiterm([2 2 1 R],A2',1)
lmiterm([2 2 1 X],1,A2)
lmiterm([2 2 1 Z],1,C)
lmiterm([2 2 1 M],1,1)
lmiterm([2 1 3 R],1,B)
lmiterm([2 4 1 0],L)
lmiterm([2 4 1 0],0)
lmiterm([2 4 1 0],-L)
lmiterm([2 2 2 X],1,A2,'s')
lmiterm([2 2 2 Z],1,C,'s')
lmiterm([2 2 3 X],1,B)
lmiterm([2 2 3 Z],1,D)
lmiterm([2 4 2 0],L)
lmiterm([2 4 2 0],0)
lmiterm([2 3 3 0],-1)
lmiterm([2 4 3 0],0)
lmiterm([2 4 4 0],-gama^2)
%********************************************************************
lmiterm([-3 1 1 R],-1,1)
lmiterm([-3 1 1 X],1,1)
lmiterm([3 1 1 0],1e-8)
%********************************************************************
lmiterm([-4 1 1 R],1,1)
%********************************************************************
lmis=getlmis
decinfo(lmis,X)
decinfo(lmis,R)
decinfo(lmis,M)
decinfo(lmis,Z)
%********************************************************************
[tmin,xfeas]=feasp(lmis,[0 0 10 0 0],-1)
%*********************************************************************
X=dec2mat(lmis,xfeas,X);
%**********************************************************************
R=dec2mat(lmis,xfeas,R);
%**********************************************************************
M=dec2mat(lmis,xfeas,M);
%***********************************************************************
%***********************************************************************
Z=dec2mat(lmis,xfeas,Z);
%***********************************************************************
%*************************************************************************
%*************************************************************************
Af=inv(R-X)*M;
Bf=inv(R-X)*Z;
%*************************************************************************
x=[0.1;0.1;0.05*Rad_Degree;0.05*Rad_Degree;0.05*Rad_Degree;0.001*zh;0.001*zh];
gjx=[0;0;0;0;0;0;0];
for i=0:1:1000
   step=0.01;
   w=[0.001*randn(5,1);0;0;0.01*randn(2,1)];
   dx=x;
   k1=A*x+B*w;   
   x1=dx+1/2*step*k1;
   k2=A*x1+B*w;
   x2=dx+1/2*step*k2;
   k3=A*x2+B*w;
   x3=dx+step*k3;
   k4=A*x3+B*w;
   k=1/6*(k1+2*k2+2*k3+k4);    
   x=x+step*k;
   y=C*x+D*w;    
   z=L*x; 
   realx3(i+1)=x(3);
   realx4(i+1)=x(4);
   realx5(i+1)=x(3)+x(4)*tan(angle);   
   step=0.01;
   [FF,GG]=c2d(Af,Bf,step);
   gjx=FF*gjx+GG*y;
   estimatex3(i+1)=gjx(3);
   estimatex4(i+1)=gjx(4);
   estimatex5(i+1)=gjx(3)+gjx(4)*tan(angle);
  end
%画图
i=0:0.01:10;
figure(1);
subplot(3,1,1);
plot(i,realx3,'b:',i,estimatex3,'r:');
grid;
ylabel('东向水平失准角');
subplot(3,1,2);
plot(i,realx4,'b:',i,estimatex4,'r:');
grid;
ylabel('北向水平失准角');
subplot(3,1,3);
plot(i,realx5,'b:',i,estimatex5,'r:');
grid;
xlabel('time(s)');
ylabel('方位失准角');