kalman_movebase_circle.m

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

%************************** psi角方程 **********
%           kalman filter
%            动基座对准 加速度计零偏 和陀螺漂移都变好了 
%           匀速 圆周运动
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;clc;
maxcount=60*8;
Ts=1;T=0.05;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%增益矩阵和噪声方差阵的设定%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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.3*pi/180/3600)^2 0 0 ;
    0   0   0 0 0 0 0 0 (0.3*pi/180/3600)^2 0 ;
    0   0   0 0 0 0 0 0 0 (0.3*pi/180/3600)^2 ];
Q=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);
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;% 东向的速度
W_circle=0.05    %  载体匀速旋转的速度 0.5弧度/秒
%*****************************************************
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=Vm*cos(W_circle*1*Ts);   Ve=Vm*sin(W_circle*1*Ts);
fn=-Vm*W_circle*sin(W_circle*Ts);fe=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.3*pi/180/3600; 0.3*pi/180/3600; 0.3*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;
p=zeros(1,maxcount);
P0=PP;p(1)=P0(5,5);
ww=0;
x1=zeros(10,maxcount);
%%%%%%%%%%%%%%%%%%%
for i=2:maxcount
    %*********************************************************************
  w=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=Vm*cos(W_circle*i*Ts);   Ve=Vm*sin(W_circle*i*Ts);
%载体坐标系  的速度 恒定速度随着圆周运动会有变化
fn=-Vm*W_circle*sin(W_circle*Ts*i);fe=Vm*W_circle*cos(W_circle*Ts*i);
% 载体坐标系的数值********************************************
%********************************   ???????????????????????   ***************这里的角度不对
   
    %*********** 经纬度的变化  ***************************************
    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;%这里有时间延迟的影响
  %****************** 滤波就用这个存在误差的模型来进行 并非实际的模型************************************************
 w=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=Vm*cos(W_circle*i*Ts);   Ve=Vm*sin(W_circle*i*Ts);
%载体坐标系  的速度 恒定速度随着圆周运动会有变化
fn=-Vm*W_circle*sin(W_circle*Ts*i);fe=Vm*W_circle*cos(W_circle*Ts*i);
% 载体坐标系的数值********************************************
    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=170;%备选项，500 700
%Q1=F/15;%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)/30;
%VV=[0.000821654 0; 0 0.000821654];%振幅大，误差可能会比较小 
%VV=[0.00821654 0; 0 0.00821654];%振幅中， 
%VV=[0.0121654 0; 0 0.0121654];%振幅小
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Pminimax=zeros(10,maxcount);
  %Qk=(Q+F*Q*F')*Ts/2;
  %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]
 % Pminimax(:,i)=P0(5,5);
%%%%%%%%%%%%% kalman filter %%%%%%%%%%%%%%%%5
  Qk=(Q+F*Q*F')*Ts/2;
  P0 = (F*P0*F'+Qk); % P[n+1|n] 
  K = P0*C'*inv(C*P0*C'+R);
  x = F*x; % x[n+1|n]
  x = x + K*(yv(:,i)-C*x);  % x[n|n]
  P0 = (eye(10)-K*C)*P0;  
  p(i)=P0(5,5);
 x1(:,i)=x;%输出为估计值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
%%%%%%%%%%%%%%%%% 显示结果 %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%输出结果%%%%%%%%%%%%%%%%%%%%%
t=[1:maxcount]';
figure(1),
plot(sqrt(p)*180/pi*60);xlabel('航向角估计方差 分');grid;  %0.01*pi/WN/180 稳态误差
figure(2);
subplot(521), plot(t,x1(1,:),'r--',t,x0(1,:),'-.');ylabel('north velocity');grid;
subplot(522), plot(t,x1(2,:),'r--',t,x0(2,:),'-.');ylabel('east velocity');grid;
subplot(523); plot(t,x0(3,:)*180/pi*60,'r--',t,x1(3,:)*180/pi*60);ylabel('north ESTIMATION');grid;
subplot(524); plot(t,x0(4,:)*180/pi*60,'r--',t,x1(4,:)*180/pi*60);ylabel('east ESTIMATION');grid;
subplot(525); plot(t,x0(5,:)*180/pi*60,'r--',t,x1(5,:)*180/pi*60);ylabel('yaw ESTIMATION');grid;
subplot(526), plot(t,x1(6,:)/0.000001,'r--',t,x0(6,:)/0.000001,'-.');ylabel('north accelerator ug');grid;
subplot(527), plot(t,x1(7,:)/0.000001,'r--',t,x0(7,:)/0.000001,'-.');ylabel('east accelerator ug');grid;
subplot(528), plot(t,x1(8,:)/0.000001,'r--',t,x0(8,:)/0.000001,'-.');ylabel('north gyro drift o/h');grid;
subplot(529), plot(t,x1(9,:)*180/pi*3600,'r--',t,x0(9,:)*180/pi*3600,'-.');ylabel('east gyro drift o/h');grid;
subplot(5,2,10), plot(t,x1(10,:)*180/pi*3600,'r--',t,x0(10,:)*180/pi*3600,'-.');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);ylabel('航向失准角误差/角分 ');xlabel('时间/秒');grid;

figure(4);
r1=x1(3,:)*180/pi;
r2=x1(4,:)*180/pi;
r3=x1(5,:)*180/pi;
%plot(r1,'g');hold on;
%plot(r2,'r');hold on;
plot(r3,'b');grid;ylabel('航向失准角估计状态/角分 ');xlabel('时间/秒');
%figure(6);plot(r3,'g');hold on;plot(r4,'r','lineWidth',2);grid;
%figure(7);
%subplot(311), plot(t,(x1(3,:)-x0(3,:))*180/pi*60,'b','lineWidth',2);ylabel('north error ');grid;hold on;plot(t,(x2(3,:))*180/pi*60,'r','lineWidth',2);
%subplot(312), plot(t,(x1(4,:)-x0(4,:))*180/pi*60,'b','lineWidth',2);ylabel('east error ');grid;hold on;plot(t,(x2(4,:))*180/pi*60,'r','lineWidth',2);
%subplot(313), plot(t,(x1(5,:)-x0(5,:))*180/pi*60,'b','lineWidth',2);ylabel('yaw error ');xlabel('估计误差');grid;hold on;plot(t,(x2(5,:))*180/pi*60,'r','lineWidth',2);
%hold off
figure(5)
subplot(211), plot(t,x1(1,:)-x0(1,:));ylabel('north velocity');grid;
subplot(212), plot(t,x1(2,:)-x0(2,:));ylabel('east velocity');grid;