modulated_jiasuline.m

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

%************************** psi角方程 **********
%           minimax filter
%            经过调制的
%          载体作 匀加速直线运动
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;clc;
maxcount=60*8;
Ts=1;T=0.05;
%**********  转动速度数据的读入  *****************************
f1=fopen('zhuanjiao.txt');
s1=fscanf(f1,' %e',inf);
fclose all;
temp=((s1/12.6-0.01)*180/pi-0.1139);
s=zeros(10000,1);
s=s1;
s((length(s1)+1):(2*length(s1)))=s1;
s((length(s)+1):(2*length(s)))=s;
s((length(s)+1):(2*length(s)))=s;
s((length(s)+1):(2*length(s)))=s;
s((length(s)+1):(2*length(s)))=s;
w1=zeros(900,1);
for j=1:60*15
    for i=1:10
       w1(j)=s(10*(j-1)+i)+w1(j);
    end
end 
w1=w1/10;% 80s  one circle
%w= 45*pi/180;%弧度/秒
w=10.47;   %理论上转动速度
%-----------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%增益矩阵和噪声方差阵的设定%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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.1*pi/180/3600)^2 0 0 ;
    0   0   0 0 0 0 0 0 (0.1*pi/180/3600)^2 0 ;
    0   0   0 0 0 0 0 0 0 (0.1*pi/180/3600)^2 ];
Q=diag([(0.00005*9.8)^2, (0.00005*9.8)^2, (.3*pi/180/3600)^2, (.3*pi/180/3600)^2, (.3*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];
B=eye(10);
%%%%%%%%%%%%%%%%%%%%%%%%%%  地球参数的设定  %%%%%%%%%%%%%%%%%%%%%%
Ww=360.0*pi/24/180/3600; %单位是弧度
L=40.0;
g=9.80;
Rearth=6378137; %地球半径
WD=-Ww*sin(L*pi/180);   
WN= Ww*cos(L*pi/180);
Wie=360.0*pi/24/180/3600; %单位是弧度
%********************************************************************
 %        第一个周期的数值
%**************************************************
Vm=2;% 东向的速度       加速度0.4m/s^2
W_circle=0;   %  载体匀速旋转的速度 0弧度/秒  因为是直线运动
%*****************************************************
w=w++W_circle;
 cbs = [cos(w*1*Ts) sin(w*1*Ts) 0; 
       -sin(w*1*Ts) cos(w*1*Ts) 0;
         0             0        1];
     csb=cbs';
     
%************************************************
Vn=0;%Vm*cos(W_circle*1*Ts);   
Ve=Vm;
fn=0;%-Vm*W_circle*sin(W_circle*Ts);
fe=0.04;%Vm*W_circle*cos(W_circle*Ts);
%载体坐标系的数值********************************************
%*********** 经纬度的变化  ***************************************
jingdu=Ve/Rearth/(cos(L*pi/180));
weidu=Vn/Rearth;
%*******************************************************************
 A=[0   -(2*Wie+jingdu)*sin(L*pi/180)   0    g    fe    csb(1,1) csb(1,2) 0 0 0;
    (2*Wie+jingdu)*sin(L*pi/180)     0   -g    0   -fn    csb(2,1) csb(2,2) 0 0 0;
    0    0    0   -(Wie+jingdu)*sin(L*pi/180)    weidu    0 0 csb(1,1) csb(1,2) csb(1,3);
     0      0   (Wie+jingdu)*sin(L*pi/180)  0 (Wie+jingdu)*cos(L*pi/180)   0 0 csb(2,1) csb(2,2) csb(2,3);
     0      0    -weidu  -(Wie+jingdu)*cos(L*pi/180) 0   0 0 csb(3,1) csb(3,2) csb(3,3);
     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);
[F1,G1,C1,Dd]=ssdata(sysd);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55
V=zeros( 2,maxcount);
W=zeros(10,maxcount);
for i=1:10
    W(i,:)=sqrt(Q(i,i))*randn(1,maxcount);
end
for i=1:2
    V(i,:)=sqrt(R(i,i))*randn(1,maxcount);
end
%%%%%%%%%%%%%%%  生成数据  %%%%%%%%%%%%%%%%%%%%%%%%%%%%
X0=[0.1; 0.1; 1*pi/180;1*pi/180;1*pi/180; 0.0001*9.80; 0.0001*9.80; 0.1*pi/180/3600; 0.1*pi/180/3600; 0.1*pi/180/3600];
%常值漂移 0.3度每小时   弧度/秒  加速度计零偏 100ug
%%%%%%%%%%%%%%%%%%%%%%%%
x0=zeros(10,maxcount);
x0(:,1)=F1*X0+W(:,1);
yv(:,1)=C1*X0+V(:,1)+[fn fe]'*T;
%%%%%%%%%%%%%%%%% Kalman Filter %%%%%%%%%%%%%%%%%%%%
x1=zeros(10,maxcount);%初始状态为零
x=zeros(10,1);   
x1(:,1)=x;x2(:,1)=x;
p=zeros(1,maxcount);p2=zeros(1,maxcount);
P0=PP;p(1)=P0(5,5);P02=PP;p2(1)=p(1);
ww=0;
x1=zeros(10,maxcount);
%%%%%%%%%%%%%%%%%%%
for i=2:maxcount
    %*********************************************************************
    w1(i)=w1(i)+W_circle;%  载体 实际上的 带着误差的     匀速旋转的速度 0.5弧度/秒
    ww=ww+w1(i);%一共转过的角度  
     cbs = [cos(ww) sin(ww) 0; 
           -sin(ww) cos(ww) 0;
            0       0       1]; 
    csb=cbs';
%********************************************************************        
Vn=0;%Vm*cos(W_circle*1*Ts);   
Ve=Vm;
fn=0;%-Vm*W_circle*sin(W_circle*Ts);
fe=0.04;%Vm*W_circle*cos(W_circle*Ts);
% 载体坐标系的数值********************************************
%********************************   ???????????????????????   ***************这里的角度不对
   
    %*********** 经纬度的变化  ***************************************
    jingdu=Ve/Rearth/(cos(L*pi/180));
    weidu=Vn/Rearth;
    %*******************************************************************
    %temp(:,i)=cbs'*[0.1*pi/180/3600;0.1*pi/180/3600;0.1*pi/180/3600];%可以从中看到经过调制的陀螺漂移
    A=[0   -(2*Wie+jingdu)*sin(L*pi/180)   0    g    fe    csb(1,1) csb(1,2) 0 0 0;
    (2*Wie+jingdu)*sin(L*pi/180)     0   -g    0   -fn    csb(2,1) csb(2,2) 0 0 0;
    0    0    0   -(Wie+jingdu)*sin(L*pi/180)    weidu    0 0 csb(1,1) csb(1,2) csb(1,3);
     0      0   (Wie+jingdu)*sin(L*pi/180)  0 (Wie+jingdu)*cos(L*pi/180)   0 0 csb(2,1) csb(2,2) csb(2,3);
     0      0    -weidu  -(Wie+jingdu)*cos(L*pi/180) 0   0 0 csb(3,1) csb(3,2) csb(3,3);
     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);
    [F1,G1,C1,Dd]=ssdata(sysd); 
    %%%%%%%%%%%%%%%%%%%生成真实数据%%%%%%%%%%%%%%%%%%%%%%%%%%%
    x0(:,i)=F1*x0(:,i-1)+W(:,i);
    yv(:,i)=C1*x0(:,i)+V(:,i)+[fn fe]'*T;%这里有时间延迟的影响
  %****************** 滤波就用这个存在误差的模型来进行 并非实际的模型************************************************
 %  载体匀速旋转的速度 0.5弧度/秒
 w=10.47++W_circle;
  cbs = [cos(w*i*Ts) sin(w*i*Ts) 0; 
        -sin(w*i*Ts) cos(w*i*Ts) 0;
         0             0        1];
csb=cbs';
%************* 速度 加速度 载体坐标系 ****************************
Vn=0;%Vm*cos(W_circle*1*Ts);   
Ve=Vm;
fn=0;%-Vm*W_circle*sin(W_circle*Ts);
fe=0.04;%Vm*W_circle*cos(W_circle*Ts);
% 载体坐标系的数值********************************************
    jingdu=Ve/Rearth/(cos(L*pi/180));
    weidu=Vn/Rearth;
    %%%%**********************************************************
A=[0   -(2*Wie+jingdu)*sin(L*pi/180)   0    g    fe    csb(1,1) csb(1,2) 0 0 0;
    (2*Wie+jingdu)*sin(L*pi/180)     0   -g    0   -fn    csb(2,1) csb(2,2) 0 0 0;
    0    0    0   -(Wie+jingdu)*sin(L*pi/180)    weidu    0 0 csb(1,1) csb(1,2) csb(1,3);
     0      0   (Wie+jingdu)*sin(L*pi/180)  0 (Wie+jingdu)*cos(L*pi/180)   0 0 csb(2,1) csb(2,2) csb(2,3);
     0      0    -weidu  -(Wie+jingdu)*cos(L*pi/180) 0   0 0 csb(3,1) csb(3,2) csb(3,3);
     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); 
%%%%%%%%%%%%%%%% 实验数据1 收敛速度比较快 但是稳态误差比较大%%%%%%%%%
%gama=10;%0.3 1 %10  0.3 0.1的时候
%Q1=F/17;%diag([0.01,0.01,0.01,0.01,0.01,0,0,0,0,0],0);;
%WW=Q/35;%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)/100;
%VV=[0.00821654 0; 0 0.00821654];%振幅中， 
%************************新的********************************************
%备选项，500 700
%Q1=PP/100;%diag([(0.00005*9.8)^2, (0.00005*9.8)^2, (1*pi/180/3600)^2, (1*pi/180/3600)^2, (1*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0)/100;
%Q1=diag([(0.001)^2, (0.001)^2, (1*pi/180/3600)^2, (1*pi/180/3600)^2, (1*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0)/15;
%初始方差矩阵
%Q1=Q/100;gama=3;% 1度随机误差
gama=1;  Q1=Q/5;%diag([(0.0000001)^2, (0.0000001)^2, (0.1*pi/180/3600)^2, (0.1*pi/180/3600)^2, (0.1*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0)/5;  %0.1度随机误差 Q/5
% 系统噪声矩阵
WW=Q/3;
%量测噪声矩阵
VV=R/3; 
  LL=inv(eye(10)-gama*Q1*P0+C'*inv(VV)*C*P0);
  K = F*P0*LL*C'*inv(VV);
  P0=F*P0*LL*F'+WW;
  P0=(P0+P0')/2;
  x1(:,i) =F*x1(:,i-1); % x[n+1|n]
  x1(:,i) =x1(:,i)+ K*(yv(:,i)-C*x1(:,i));  % x[n|n]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 p(i)=P0(5,5);
 % Pminimax(:,i)=P0(5,5);
%%%%%%%%%%%%% kalman filter %%%%%%%%%%%%%%%%5
  Qk=(Q+F*Q*F')*Ts/2;
  P02 = (F*P02*F'+Qk); % P[n+1|n] 
  K2 = P02*C'*inv(C*P02*C'+R);
  x = F*x; % x[n+1|n]
  x = x + K2*(yv(:,i)-C*x);  % x[n|n]
  P02 = (eye(10)-K2*C)*P02;  
  p2(i)=P02(5,5);
 x2(:,i)=x;%输出为估计值

end
%%%%%%%%%%%%%%%%% 显示结果 %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%输出结果%%%%%%%%%%%%%%%%%%%%%
t=[1:maxcount]';
plot(t,sqrt(p)*180/pi*60,t,sqrt(p2)*180/pi*60,'r-.', 'LineWidth',2);
xlabel('时间/秒');
ylabel('方位失准角估计误差方差 /角分');grid; legend('Hinfinity滤波','kalman滤波'); %0.01*pi/WN/180 稳态误差
figure(2);
subplot(521); plot(t,x1(1,:)-x0(1,:),t,x2(1,:)-x0(1,:),'r', 'LineWidth',2);ylabel('north velocity');grid;legend('minimax','kalman');
subplot(522); plot(t,x1(2,:)-x0(2,:),t,x2(2,:)-x0(2,:),'r', 'LineWidth',2);ylabel('east velocity');grid;
subplot(523); plot(t,(x0(3,:)-x1(3,:))*180/pi*60,t,(x0(3,:)-x2(3,:))*180/pi*60,'r', 'LineWidth',2);ylabel('north ESTIMATION');grid;
subplot(524); plot(t,(x0(4,:)-x1(4,:))*180/pi*60,t,(x0(4,:)-x2(4,:))*180/pi*60,'r', 'LineWidth',2);ylabel('east ESTIMATION');grid;
subplot(525); plot(t,(x0(5,:)-x1(5,:))*180/pi*60,t,(x0(5,:)-x2(5,:))*180/pi*60,'r', 'LineWidth',2);ylabel('yaw ESTIMATION');grid;
subplot(526), plot(t,(x1(6,:)-x0(6,:))/0.000001,t,(x2(6,:)-x0(6,:))/0.000001,'r', 'LineWidth',2);ylabel('north accelerator ug');grid;
subplot(527), plot(t,(x1(7,:)-x0(7,:))/0.000001,t,(x2(7,:)-x0(7,:))/0.000001,'r', 'LineWidth',2);ylabel('east accelerator ug');grid;
subplot(528), plot(t,(x1(8,:)-x0(8,:))/0.000001,t,(x2(8,:)-x0(8,:))/0.000001,'r', 'LineWidth',2);ylabel('north gyro drift o/h');grid;
subplot(529), plot(t,x1(9,:)*180/pi*3600-x0(9,:)*180/pi*3600,t,x2(9,:)*180/pi*3600-x0(9,:)*180/pi*3600,'r', 'LineWidth',2);ylabel('east gyro drift o/h');grid;
subplot(5,2,10), plot(t,x1(10,:)*180/pi*3600-x0(10,:)*180/pi*3600,t,x2(10,:)*180/pi*3600-x0(10,:)*180/pi*3600,'r', 'LineWidth',2);ylabel('yaw gyro drift o/h');xlabel('估计状态与真实状态');grid;
%**************************************************************************
figure(3);
%subplot(311), plot(t,(x1(3,:)-x0(3,:))*180/pi*60);ylabel('north error ');grid;
%subplot(312), plot(t,(x1(4,:)-x0(4,:))*180/pi*60);ylabel('east error ');grid;
%subplot(313), plot(t,(x1(5,:)-x0(5,:))*180/pi*60);ylabel('yaw error ');xlabel('估计误差');grid;
plot(t,(x1(5,:)-x0(5,:))*180/pi*60,t,(x2(5,:)-x0(5,:))*180/pi*60,'r', 'LineWidth',2);
ylabel('航向失准角误差/角分 ');xlabel('时间/秒');grid;
legend('minimax','kalman');
%****************************************************************
figure(4);
subplot(211);plot(t,x1(1,:)-x0(1,:),t,x2(1,:)-x0(1,:),'r', 'LineWidth',2);
ylabel('北向速度误差 米/秒 ');
xlabel('时间 /秒');
grid;
legend('minimax filter','kalman filter');
subplot(212);plot(t,x1(2,:)-x0(2,:),t,x2(2,:)-x0(2,:),'r', 'LineWidth',2);
%subplot(212);plot(t,(x_kalman(2,:)-0.4),'r-.',t,(x_minimax(2,:)-0.44), 'LineWidth',2);%匀速圆周运动
ylabel('东向速度误差 米/秒 ');
xlabel('时间 /秒');
grid;
legend('minimax filter','kalman filter');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%(x0(3,:)-x1(3,:))*180/pi*60
%(x0(3,:)-x2(3,:))*180/pi*60
%(x0(4,:)-x1(4,:))*180/pi*60
%(x0(4,:)-x2(4,:))*180/pi*60
%plot(r1,'g');hold on;
%plot(r2,'r');hold on;
%plot(t,x1(5,:)*180/pi,t,x2(5,:)*180/pi,'r', 'LineWidth',2);grid;ylabel('航向失准角估计状态 /角分 ');xlabel('时间/秒');legend('minimax','kalman');
%%f1=fopen('temp_minimaxjiasu.txt','w');
%fprintf(f1,' %e  %e  %e  %e  %e  %e  %e  %e  %e  %e',x1-x0);
%f1=fopen('temp_minimaxfangcha.txt','w');
%fprintf(f1,' %e ',sqrt(p)*180/pi*60);
%fclose all;
figure;
subplot(411); plot(t,x1(1,:)-x0(1,:),t,x2(1,:)-x0(1,:),'r', 'LineWidth',2);ylabel('north velocity');grid;legend('minimax','kalman');
subplot(412); plot(t,x1(2,:)-x0(2,:),t,x2(2,:)-x0(2,:),'r', 'LineWidth',2);ylabel('east velocity');grid;
subplot(413); plot(t,(x0(3,:)-x1(3,:))*180/pi*60,t,(x0(3,:)-x2(3,:))*180/pi*60,'r', 'LineWidth',2);ylabel('north ESTIMATION');grid;
subplot(414); plot(t,(x0(4,:)-x1(4,:))*180/pi*60,t,(x0(4,:)-x2(4,:))*180/pi*60,'r', 'LineWidth',2);ylabel('east ESTIMATION');grid;