kalmansample.m

一个很好的卡尔曼滤波程序呵呵

 function kalmansample
%  x=[584677 193930 -6.803 -2.468 ]';%飞行器初始的位置以及速度，以金星为原点，对称于飞行器双曲线飞行轨道建立xy坐标系，一颗恒星位于
 x=[165330 42402 -7.4 -2.66 ]';
 x2=x;%带噪声的飞行器的初始状态

 t=30;%采样的周期
 count=900;%采样的点数
%  t=100;%采样的周期
%  count=1000;%采样的点数 
 u=3.253e5;%行星的引力常数
 r=6.05e3;%行星的半径
 suiji=randn(6*count);%产生随机向量[0~1]
 
for i=1:count        %采样的点数
    xx(i)=x(1);       %x坐标的记录       
    yy(i)=x(2);       %y坐标的记录
    xx2(i)=x2(1);   %带噪声的x坐标的记录
    yy2(i)=x2(2);   %带噪声的y坐标的记录
    vx(i)=x(3);     %x方向上的速度
    vy(i)=x(4);     %y方向上的速度    
    eex(i)=xx2(i)-xx(i);%x方向上的误差
    eey(i)=yy2(i)-yy(i);%y方向上的误差
    
    rand('seed',0);%产生随机数
    %以下部分形成观测角，即飞行器的矢量相对x轴转过的角度（0~360度）假设一颗恒星在x轴方向上，2维平面只要一颗恒星即可确定飞行器的方向：
    
    q=3e-4*suiji(i);
    if(xx(i)<0)%2，3象限的情况
      j1(i)=atand(yy(i)/xx(i))+180;%认为轨道没有误差,观测角没有误差
      j2(i)=atand(yy2(i)/xx2(i))+180;%认为轨道有误差,观测角没有误差
      j3(i)=atand(yy(i)/xx(i))+180+q;%认为轨道没有误差,观测角有误差
      j4(i)=atand(yy2(i)/xx2(i))+180+q;%认为轨道有误差,观测角也有误差
    elseif(xx(i)>=0&yy(i)<0)
      j1(i)=atand(yy(i)/xx(i))+360;%认为轨道没有误差,观测角没有误差
      j2(i)=atand(yy2(i)/xx2(i))+360;%认为轨道有误差，观测角没有误差
      j3(i)=atand(yy(i)/xx(i))+360+q;%认为轨道没有误差,观测角有误差
      j4(i)=atand(yy2(i)/xx2(i))+360+q;%认为轨道有误差，观测角也有误差    
    else
      j1(i)=atand(yy(i)/xx(i));   %认为轨道没有误差,观测角没有误差
      j2(i)=atand(yy2(i)/xx2(i));%认为轨道有误差，观测角没有误差
      j3(i)=atand(yy(i)/xx(i));%认为轨道没有误差,观测角有误差
      j4(i)=atand(yy2(i)/xx2(i))+q;%认为轨道有误差，观测角也有误差
    end
    
%      q2=5e-3*((suiji(count+i)-0.5)*2);
       q2=5e-3*suiji(count+i); 
      jj1(i)=2*asind(r/sqrt(xx(i)^2+yy(i)^2));%对行星的张角测量,认为轨道没有误差，观测角没有误差
      jj2(i)=2*asind(r/sqrt(xx2(i)^2+yy2(i)^2));%对行星的张角测量,认为轨道有误差，观测角没有误差
      jj3(i)=2*asind(r/sqrt(xx(i)^2+yy(i)^2))+q2;%对行星的张角测量,认为轨道没有误差，观测角有误差
      jj4(i)=2*asind(r/sqrt(xx2(i)^2+yy2(i)^2))+q2;%对行星的张角测量,认为轨道有误差，观测角有误差
      
      
     k1=u*(2*x(1)^2-x(2)^2)*t/(sqrt(x(1)^2+x(2)^2))^5;
     k2=3*u*x(1)*x(2)*t/(sqrt(x(1)^2+x(2)^2))^5;
     k3=3*u*x(1)*x(2)*t/(sqrt(x(1)^2+x(2)^2))^5;
     k4=u*(2*x(2)^2-x(1)^2)*t/(sqrt(x(1)^2+x(2)^2))^5;
         
     A=[1 0  t 0 ;%状态转移矩阵
        0 1  0 t ;
        -k1 -k2  1 0 ;
        -k3 -k4  0 1 ;];
     A2=[1 0  t 0 ;%带噪声的状态转移矩阵
         0 1  0 t ;
         -u*(3*x2(1)^2-(x2(1)^2+x2(2)^2))*t/(sqrt(x2(1)^2+x2(2)^2))^5 -3*u*x2(1)*x2(2)*t/(sqrt(x2(1)^2+x2(2)^2))^5  1 0 ;
         -3*u*x2(1)*x2(2)*t/(sqrt(x2(1)^2+x2(2)^2))^5 -u*(3*x2(2)^2-((x2(1)^2+x2(2)^2)))*t/(sqrt(x2(1)^2+x(2)^2))^5 0 1 ;];
         x=A*x;
    q=[2e-4*suiji(2*count+i) 2e-4*suiji(3*count+i)  2e-6*suiji(4*count+i) 2e-6*suiji(5*count+i)]';
    qq(i)=q(1);   %位置x的随机数
    x2=A2*x2+q;   %生成带噪声的随机向量  
end


i=1:count;
figure(1)
plot(xx,yy);
% figure(2)
% plot(i,vx);
% figure(3)
% plot(i,vy);

% 以下程序用观测数据进行kalman滤波

% xk=[584677 193930 -6.803 -2.468]';
 xk=[165000 42402 -7.4 -2.66]';
% xk=[0 0 0 0]';
xk_1=xk;
% pk=[2000000 0 0 0;
%     0 60000 0 0;
%     0 0 1e-6 0;
%     0 0 0 1e-6;];
%pk=cov((xk-x)*(xk-x)')；
 pk=diag([100000 0 0 0]);
temp=randn(5*count,2);
temp2=randn(3*count,2);

for k=2:count+1
    xk_1=xk;
    f=[xk_1(3) xk_1(4) -u*xk_1(1)/(sqrt(xk_1(1)^2+xk_1(2)^2))^3 -u*xk_1(2)/(sqrt(xk_1(1)^2+xk_1(2)^2))^3 ]';
     xk_k_1=xk_1+f*t;
    
     k1=u*(2*xk_k_1(1)^2-xk_k_1(2)^2)*t/(sqrt(xk_k_1(1)^2+xk_k_1(2)^2))^5;
     k2=3*u*xk_k_1(1)*xk_k_1(2)*t/(sqrt(xk_k_1(1)^2+xk_k_1(2)^2))^5;
     k3=3*u*xk_k_1(1)*xk_k_1(2)*t/(sqrt(xk_k_1(1)^2+xk_k_1(2)^2))^5;
     k4=u*(2*xk_k_1(2)^2-xk_k_1(1)^2)*t/(sqrt(xk_k_1(1)^2+xk_k_1(2)^2))^5;
         
     A=[1 0  t 0 ;%状态转移矩阵,这里加不加负号呢啊
        0 1  0 t ;
        -k1 -k2  1 0 ;
        -k3 -k4  0 1 ;];
%     q=diag([4e-8*temp(4*k) 4e-8*temp(4*k+1) 4e-12*temp(4*k+2) 4e-12*temp(4*k+3)]);
   q=diag([0 0 0 0]);
    pk_k_1=A*pk*A'+q;
%     用余弦求夹角的
%     h11=-abs(xk_k_1(2)/(xk_k_1(1)^2+xk_k_1(2)^2));
%     h12=-xk_k_1(1)*xk_k_1(2)/(abs(xk_k_1(2))*(xk_k_1(1)^2+xk_k_1(2)^2));

%     用正切求夹角的
    h11=-xk_k_1(2)/(xk_k_1(1)^2+xk_k_1(2)^2);
    h12=xk_k_1(1)/(xk_k_1(1)^2+xk_k_1(2)^2);
    
    h21=-2*r*xk_k_1(1)/(sqrt(xk_k_1(1)^2+xk_k_1(2)^2-r^2)*(xk_k_1(1)^2+xk_k_1(2)^2)); 
    h22=-2*r*xk_k_1(2)/(sqrt(xk_k_1(1)^2+xk_k_1(2)^2-r^2)*(xk_k_1(1)^2+xk_k_1(2)^2));
    
%     h11=-xk_1(2)/(xk_1(1)^2+xk_1(2)^2);
%     h12=xk_1(1)/(xk_k_1(1)^2+xk_1(2)^2)
%     
%     h21=-2*r*xk_1(1)/((sqrt(xk_1(1)^2+xk_1(2)^2-r^2)*(xk_1(1)^2+xk_1(2)^2))); 
%     h22=-2*r*xk_1(2)/((sqrt(xk_1(1)^2+xk_1(2)^2-r^2)*(xk_1(1)^2+xk_1(2)^2)));
    H=[h11 h12 0 0 ;                  
       h21 h22 0 0 ;];
   
    %R=diag([5e-3 5e-3]).*temp(k,1);
    R=diag([9e-8 25e-6]);
%     R=diag([0 0]);
    kk=pk_k_1*H'*inv([H*pk_k_1*H'+R]);
    
%     h=[acosd(xk_k_1(1)/sqrt(xk_k_1(1)^2+xk_k_1(2)^2))  2*asind(r/sqrt(xk_k_1(1)^2+xk_k_1(2)^2))]';
    h=[atand(xk_k_1(2)/xk_k_1(1)) 2*asind(r/sqrt(xk_k_1(1)^2+xk_k_1(2)^2))]';
    htest1(k-1)=h(1);
    htest2(k-1)=h(2);
    zk=[j4(k-1) jj4(k-1)]';
    zk-h
    xk=xk_k_1+kk*[zk-h];
    jilu(k-1)=xk(1);
 %  pk=(diag(1)-kk*H)*pk_k_1*(diag(1)-kk*H)'+kk*R*kk';  %这里用扩展kalman滤波的公式是哪个？
       pk=(diag(1)-kk*H)*pk_k_1;
end

 i=1:count;
 figure(1)
 plot(xx,yy)
 ylabel('y坐标');
 xlabel('x坐标');
 figure(2)
 plot(i,j1);
 ylabel('对恒星的观测角');
 xlabel('时间');
 figure(3)
 plot(i,jj1)
 ylabel('对行星的张角');
 xlabel('时间');
 figure(5)
 plot(i,xx)
 ylabel('x坐标');
 xlabel('时间');
 figure(6)
 plot(i,htest1)
 ylabel('计算过程中对恒星的测量角');
 xlabel('时间');
 
 figure(7)
 plot(i,htest2)
 ylabel('计算过程中对行星的张角');
 xlabel('时间');
 
 figure(8);
 plot(i,jilu)
 ylabel('估计的x坐标');
 xlabel('时间');

 figure(9);
 plot(i,jilu-xx);
 ylabel('估计与实际x的偏差');
 xlabel('时间');