kalmansample2.m

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

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

%  t=3;%采样的周期
%  count=900;%采样的点数
 t=100;%采样的周期
 count=10000;%采样的点数 
 u=6.253e5;%行星的引力常数
 r=6.05e3;%行星的半径
 
for i=1:count        %采样的点数
    deltax=x1(1)-x(1);
    deltay=x1(2)-x(2);
    deltavx=x1(3)-x(3);
    deltavy=x1(4)-x(4);
    delta=[deltax deltay deltavx deltavy]';
    xx(i)=x(1);       %x坐标的记录       
    yy(i)=x(2);       %y坐标的记录
    vx(i)=x(3);     %x方向上的速度
    vy(i)=x(4);     %y方向上的速度 
    
    k1=u*(2*x1(1)^2-x1(2)^2)*t/(sqrt(x1(1)^2+x1(2)^2))^5;
    k2=3*u*x1(1)*x1(2)*t/(sqrt(x1(1)^2+x1(2)^2))^5;
    k3=3*u*x1(1)*x1(2)*t/(sqrt(x1(1)^2+x1(2)^2))^5;
    k4=u*(2*x1(2)^2-x1(1)^2)*t/(sqrt(x1(1)^2+x(2)^2))^5;
         
    A=[1 0  t 0 ;%状态转移矩阵
       0 1  0 t ;
       k1 k2 1 0 ;
       k3 k4 0 1 ;];
    x=x1;
    x1=A*delta+x1;
   
end


i=1:length(xx);
figure(1)
plot(xx,yy);