dhfz3.m

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

%鲁棒滤波
clc
clear all;
close all;
format long;
g=9.7803;
wie=7.29e-5;
Rad_Degree=1/180.0*3.14; % “度”转为“弧度”
zh=1/180*3.14/3600;% “度/小时”转换成“弧度/秒”
phi = 45*Rad_Degree;
RR=6375*1000;
x=zeros(7,1);
xx=zeros(7,1);
Wd=wie*sin(phi);
Wn=wie*cos(phi);
A1=[0 2*Wd 0 0 -g 0;
    -2*Wd 0 0 g 0 0;
    0 1/RR 0 0 0 0;
    0 -1/RR 0 0 Wd -Wn;
    1/RR 0 -Wd -Wd 0 0;
    1/RR*tan(phi) 0 Wn Wn 0 0];
G1=[1 0 0 0 0 0;
   0 1 0 0 0 0;
   0 0 1 0 0 0;
   0 0 0 1 0 0;
   0 0 0 0 1 0;
   0 0 0 0 0 1];
C1=[1 0 0 0 0 0;
    0 1 0 0 0 0];
step=0.01;
[F,G]=c2d(A1,G1,step);
D1=eye(2);
L1=[0 0 0 0.1 0 0;
    0 0 0 0 0.1 0];

r=0.3;
%--------------solve riccati equation-------------
%Q=B'*B;
A=A1';
B1=L1';
B2=C1';
C=G1';
B=[B1,B2];
m1=size(B1,2);
m2=size(B2,2);
R=[-r^2*eye(m1) zeros(m1,m2);zeros(m2,m1) eye(m2)];
P=care(A,B,C'*C,R);
KK=P*C1';
%---------------------------当w和v是白噪声时--------------------------------------
x=[0.1;0.1;0.05*Rad_Degree;0.05*Rad_Degree;0.05*Rad_Degree;0.1*Rad_Degree];
gjx=[0;0;0;0;0;0];
for i=0:1:10000
   step=0.01;
   w=0.001*randn(6,1);
   v=0.001*randn(2,1);
   dx=x;
   k1=A1*x+G1*w;   
   x1=dx+1/2*step*k1;
   k2=A1*x1+G1*w;
   x2=dx+1/2*step*k2;
   k3=A1*x2+G1*w;
   x3=dx+step*k3;
   k4=A1*x3+G1*w;
   k=1/6*(k1+2*k2+2*k3+k4);    
   x=x+step*k;
   y=C1*x+D1*v;    
   z=L1*x; 
   realx4(i+1)=x(4);
   realx5(i+1)=x(5);
   realx6(i+1)=x(6);   
   step=0.01;
   PP=A1-KK*C1;
   QQ=KK*y;
   [FF,GG]=c2d(PP,KK,step);
   gjx=FF*gjx+GG*y;
   estimatex4(i+1)=gjx(4);
   estimatex5(i+1)=gjx(5);
   estimatex6(i+1)=gjx(6);
  end
%画图
i=0:0.01:100;
figure(1);
subplot(3,1,1);
plot(i,realx4/Rad_Degree*60,'b:',i,estimatex4/Rad_Degree*60,'r:');
grid;
ylabel('东向水平失准角');
subplot(3,1,2);
plot(i,realx5/Rad_Degree*60,'b:',i,estimatex5/Rad_Degree*60,'r:');
grid;
ylabel('北向水平失准角');
subplot(3,1,3);
plot(i,realx6/Rad_Degree*60,'b:',i,estimatex6/Rad_Degree*60,'r:');
grid;
xlabel('time(s)');
ylabel('方位失准角');