kalman_unmodulat_linemovement.m

自己编写的自适应滤波算法的应用

%************************************
%  copyright by Gao,YN 
%  minimax filter algorithm 
%   kalman filter 比较结果
% 谈不上是动机座对准 但是可以看一下minima的效果
%  未经调制的 直线加速运动
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
clc;
maxcount=60*8;
Ts=1;
PP=[0.1^2   0 0 0 0 0 0 0 0 0 ;
    0   0.1^2 0 0 0 0 0 0 0 0 ;
    0   0   (1*pi/180)^2 0 0 0 0 0 0 0 ;
    0   0   0 (1*pi/180)^2 0 0 0 0 0 0 ;
    0   0   0 0 (1*pi/180)^2 0 0 0 0 0 ;
    0   0   0 0 0 (0.0001*9.8)^2 0 0 0 0 ;
    0   0   0 0 0 0 (0.0001*9.8)^2 0 0 0 ;
    0   0   0 0 0 0 0 (0.02*pi/180/3600)^2 0 0 ;
    0   0   0 0 0 0 0 0 (0.02*pi/180/3600)^2 0 ;
    0   0   0 0 0 0 0 0 0 (0.02*pi/180/3600)^2 ];
%diag([0.1^2,0.1^2,(1*pi/180)^2,(1*pi/180)^2,(1*pi/180)^2,(0.0001*9.8)^2,(0.0001*9.8)^2,(0.02*pi/180/3600)^2,(0.02*pi/180/3600)^2,(0.02*pi/180/3600)^2],0);
Q=diag([(0.00005*9.8)^2, (0.00005*9.8)^2, (0.01*pi/180/3600)^2, (0.01*pi/180/3600)^2, (0.01*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0);
R=diag([0.1*0.1,0.1*0.1],0);
H=[1 0 0 0 0 0 0 0 0 0;
   0 1 0 0 0 0 0 0 0 0];
Ww=360.0*pi/24/180/3600; %单位是PAI
L=45.0;
WD=-Ww*sin(L*pi/180);   
WN= Ww*cos(L*pi/180);
g=9.80;
B=eye(10);
A=[0     2*WD  0   g  0.4   1 0 0 0 0;
 -2*WD    0   -g   0  0   0 1 0 0 0;
   0      0    0   WD 0   0 0 1 0 0;
   0      0   -WD  0 WN   0 0 0 1 0;
   0      0    0  -WN 0   0 0 0 0 1;
   0 0 0 0 0              0 0 0 0 0;
   0 0 0 0 0              0 0 0 0 0;
   0 0 0 0 0              0 0 0 0 0;
   0 0 0 0 0              0 0 0 0 0;
   0 0 0 0 0              0 0 0 0 0];
sys=ss(A,B,H,0);
sysd=c2d(sys,Ts);
[F,G,C,Dd]=ssdata(sysd);
%%%%%%%%%%%%%%%%摄动模型%%%%%%%%%%%%%%%%%%%%%%%%%%55
A2=1*A;%如果取不为1的数 就是加入了摄动
Q2=1.5*Q;
R2=1.5*R;
sys1=ss(A2,B,C,0);
sysd1=c2d(sys1,1);
[F2,G2,C,DD]=ssdata(sysd1);
V=rand( 2,maxcount);
W=rand(10,maxcount);
for i=1:10
    W(i,:)=sqrt(Q2(i,i))*W(i,:);
end
for i=1:2
    V(i,:)=sqrt(R2(i,i))*V(i,:);
end
%%%%%%%%%%%%%%%  生成数据  %%%%%%%%%%%%%%%%%%%%%%%%%%%%
X0=[0.1; 0.1; 1*pi/180;1*pi/180;1*pi/180; 0.0001*9.80; 0.0001*9.80; 0.02*pi/180/3600; 0.02*pi/180/3600; 0.02*pi/180/3600];
%%%%%%%%%%%%%%%%%%%%%%%%
x0=zeros(10,maxcount);
x0(:,1)=X0+W(:,1);
yv(:,1)=C*X0+V(:,1);
for i=2:maxcount
x0(:,i)=F2*x0(:,i-1)+W(:,i);
yv(:,i)=C*x0(:,i)+V(:,i)+[0.4 0]';%这里加入了时间延迟
end
%%%%%%%%%%%%%%%%% Kalman Filter %%%%%%%%%%%%%%%%%%%%
x1=zeros(10,maxcount);
x=zeros(10,1);   
x1(:,1)=x;P=PP;
Pkalman=zeros(10,maxcount);
for i=2:maxcount
  P = (F*P*F'+G*Q*G'); % P[n+1|n] 
  K = P*C'*inv(C*P*C'+R);
  x = F*x; % x[n+1|n]
  x = x + K*(yv(:,i)-C*x);  % x[n|n]
  P = (eye(10)-K*C)*P;  
  % P[n|n]
  %P=(P+P')/2;
  Pkalman(:,i)=P(5,5);
  x1(:,i)=x;% X1 KALMAN FILTER的输出
end
%%%%%%%%%%%%%%%%% Minimax Filter %%%%%%%%%%%%%%%%%%%%%
P0=PP;
gama=10;%备选项，500 700
Q1=F/20;%diag([0.01,0.01,0.01,0.01,0.01,0,0,0,0,0],0);;
WW=diag([(0.00005*9.8)^2, (0.00005*9.8)^2, (0.01*pi/180/3600)^2, (0.01*pi/180/3600)^2, (0.01*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0)/20;
VV=[0.00821654 0,
    0 0.00821654];%调解了振幅
x2=zeros(10,maxcount);
x2(:,1)=zeros(10,1);;
gama1=gama;
P0=PP;%%%%%好像和收敛的时间有关系
Pminimax=zeros(10,maxcount);
for i=2:maxcount
  L=inv(eye(10)-gama1*Q1*P0+C'*inv(VV)*C*P0);
  K = F*P0*L*C'*inv(VV);
  P0=F*P0*L*F'+WW;
  %P0=(P0+P0')/2;
  lamda=eig(P0);
  if(abs(lamda(3))>=1)|(abs(lamda(4))>=1)|(abs(lamda(5))>=1)
      disp('gama is too large');
      return;
  end
  x2(:,i) =F*x2(:,i-1); % x[n+1|n]
  x2(:,i) =x2(:,i)+ K*(yv(:,i)-C*x2(:,i));  % x[n|n]
  Pminimax(:,i)=P0(5,5);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=[1:maxcount]';
%subplot(311); plot(t,x0(3,:)*180/pi*60,'b-.',t,x1(3,:)*180/pi*60,t,x2(3,:)*180/pi*60,'r');grid;
%subplot(312); plot(t,x0(4,:)*180/pi*60,'b-.',t,x1(4,:)*180/pi*60,t,x2(4,:)*180/pi*60,'r');grid;
%subplot(313); plot(t,x0(5,:)*180/pi*60,'b-.',t,x1(5,:)*180/pi*60,t,x2(5,:)*180/pi*60,'r');grid;
%xlabel('kalman filter')

k1=((x0(3,:)-x1(3,:))*180*60/pi);%单位是分
k2=((x0(4,:)-x1(4,:))*180*60/pi);
k3=((x0(5,:)-x1(5,:))*180*60/pi);
h1=((x0(3,:)-x2(3,:))*180*60/pi);
h2=((x0(4,:)-x2(4,:))*180*60/pi);
h3=((x0(5,:)-x2(5,:))*180*60/pi);

%subplot(321); plot(t,0,'black',t,k1,t,h1,'r');xlabel('北向误差角估计误差t/s');ylabel('单位分');grid;
%subplot(323); plot(t,0,'black',t,k2,t,h2,'r');xlabel('东向误差角估计误差t/s');ylabel('单位分');grid;
%subplot(325); plot(t,0,'black',t,k3,t,h3,'r');xlabel('方位误差角估计误差t/s');ylabel('单位分');grid;

%subplot(321); plot(t,h1,'r',t,k1);xlabel('北向误差角估计误差t/s');ylabel('单位分');grid;subplot(323); plot(t,h2,'r',t,k2);xlabel('东向误差角估计误差t/s');ylabel('单位分');grid;
%subplot(325); plot(t,k3,t,h3,'r');xlabel('方位误差角估计误差t/s');ylabel('单位分');grid;


%subplot(322); plot(t,k1);xlabel('北向误差角估计误差t/s');ylabel('单位分');grid; 
%subplot(324); plot(t,k2);xlabel('东向误差角估计误差t/s');ylabel('单位分');grid;
%subplot(326); plot(t,k3);xlabel('方位误差角估计误差t/s');ylabel('单位分');grid;

%figure;subplot(311); plot(K(3,:));subplot(312); plot(K(4,:));subplot(313); plot(K(5,:));    'LineWidth',2)
figure(1);
subplot(311);
plot(t,h1,'r-',t,k1,'b','LineWidth',2);xlabel('北向误差角估计误差t/s');ylabel('单位分');grid;
legend('minimax','kalman')
subplot(312);
plot(t,h2,'r-',t,k2,'b','LineWidth',2);xlabel('北向误差角估计误差t/s');ylabel('单位分');grid;
legend('minimax','kalman')
subplot(313);
plot(t,h3,'r-',t,k3,'b','LineWidth',2);xlabel('方位误差角估计误差t/s');ylabel('单位分');grid;
legend('minimax','kalman')
%figure;plot(t,k3);xlabel('Kalman Filter (t/s)');ylabel('方位角估计误差  单位分');grid;
figure(2);
plot(Pkalman(5,:)*180*60/pi);ylabel('Pkalman');hold on;grid;
plot(Pminimax(5,:)*180*60/pi,'r-');