dhfz2.m

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

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,5)];
%*********************************************
C=c;
%********************************************
gama=0.3;
L=zeros(2,7);
L(1,3)=0.001;
L(2,4)=0.001;
%******************************************************************
D=[zeros(5,7) eye(5)];
%--------------solve riccati equation-------------
AA=A';
BB=[L' C'];
Q=B*B';
m1=size(L',2);
m2=size(C',2);
R=[-gama^2*eye(m1) zeros(m1,m2);zeros(m2,m1) eye(m2)];
P=care(AA,BB,Q,R);
K=P*C';
%---------------------------当w和v是白噪声时--------------------------------------
x=[0;0;0.05*Rad_Degree;0.05*Rad_Degree;0.05*Rad_Degree;0.1*zh;0.1*zh];
gjx=[0;0;0;0;0;0;0];
for i=0:1:10000
   step=0.01;
   w=[0.0005*randn(2,1);0.01*zh*randn(3,1);0;0;0.001*randn(5,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(6);
   realx4(i+1)=x(7);
   realx5(i+1)=x(5);   
   step=0.01;
   PP=A-K*C;
   [FF,GG]=c2d(PP,K,step);
   gjx=FF*gjx+GG*y;
   estimatex3(i+1)=gjx(6);
   estimatex4(i+1)=gjx(7);
   estimatex5(i+1)=gjx(5);
  end
%画图
i=0:0.01:100;
figure(1);
subplot(3,1,1);
plot(i,realx3/zh,'b:',i,estimatex3/zh,'r:');
grid;
ylabel('东向水平失准角');
subplot(3,1,2);
plot(i,realx4/zh,'b:',i,estimatex4/zh,'r:');
grid;
ylabel('北向水平失准角');
subplot(3,1,3);
plot(i,realx5,'b:',i,estimatex5,'r:');
grid;
xlabel('time(s)');
ylabel('方位失准角');