extend_kalman.m

扩展KALMAN滤波方面的应用

%function extend_kalman(timenum) %推广的kalman filter
clear all;
close all;
t=1;
l_x=[1 t t^2/2;  %state converse matrix
    0 1 t;
    0 0 1];
l_y=l_x;
l_z=l_x;
%l=[l_x,l_y,l_z];
%l=diag(l);
zero_value=zeros(3);
l=[l_x zero_value zero_value;
    zero_value l_y zero_value;
    zero_value zero_value l_z];

g_x=[t^3/6 t^2/2 t]'; %disturb matrix
g_y=g_x;
g_z=g_x;
%g=[g_x,g_y,g_z];
%g=diag(g);
zero_value=zeros(3,1);
g=[g_x' g_y' g_z']';

w_xerror=0.1;   %state noise cov
w_yerror=0.1;
w_zerror=0.1;
%Q=0.1*rand(1);
%w_error=g*Q*g';%*[w_xerror^2 w_yerror^2 w_zerror^2]';
%zero_value1=zeros(3);
%w_error=[w_error zero_value1 zero_value1;
%     zero_value1 w_error zero_value1;
%     zero_value1 zero_value1 w_error];

r_spaceerror=10; %observate noise cov
r_azimutherror=1;
r_elevationerror=1;
r_error=[r_spaceerror r_azimutherror r_elevationerror];
r_error=diag(r_error);

%x0=[-10 10 7 50 -10 -7 3 0 0]';  %intinal state value

%x=x0(1);  %直角坐标和球面坐标的转换
%y=x0(3);
%z=x0(5);


distance=[100 80 150];  %give 3 numbers observation data :距离 方位角 仰角
azimuth=[1.307 1.700 0.994];
elevation=[-1.146 -1.246 -0.070];
 for i=1:3
    x(i)=distance(i)*cos(azimuth(i))*cos(elevation(i));
    y(i)=distance(i)*sin(azimuth(i))*cos(elevation(i));
    z(i)=distance(i)*sin(elevation(i));
 end;


ob_distance_error=100^2;
ob_azimuth_error=2.5^2;
ob_elevation_error=2.5^2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%下面这段先删去
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
filter_distance_error=150^2;
filter_atimuth_error=3^2;
filter_elevation_error=3^2;
R1=[filter_distance_error filter_atimuth_error filter_elevation_error];
R2=[filter_distance_error filter_atimuth_error filter_elevation_error];
R3=[filter_distance_error filter_atimuth_error filter_elevation_error];
R=[filter_distance_error filter_atimuth_error filter_elevation_error filter_distance_error filter_atimuth_error filter_elevation_error filter_distance_error filter_atimuth_error filter_elevation_error];
R=diag(R);
%x00=[x3 (3*(x3-x2)-(x2-x1))/t (x3-2*x2+x1)/t^2 y3 (3*(y3-y2)-(y2-y1))/t (y3-2*y2+y1)/t^2 z3 (3*(z3-z2)-(z2-z1))/t (z3-2*z2+z1)/t^2]';
x00=[0 1 0.1 0 1 0.1 0 1 0.1]';
p0=[10 1 0.05 10 1 0.05 10 1 0.05];
p00=diag(p0);
 %==================whole circle===========
 for time=1:1000
     Q=0.1*rand(1);
     w_error=g*Q*g';
     shuchu_error=[1*rand(1) 0.1*rand(1) 0.1*rand(1)]';
     %observation=[100*rand(1) 10*rand(1) 10*randn(1)];
     x_previous=l*x00;
     x_previous_store(:,time)=x_previous; %store x_estimate
     w_real=g*Q;
     x_real=l*x00+w_real;
     x_real_store(:,time)=x_real;
   xx=x_real(1);
   yy=x_real(4);
   zz=x_real(7);
   ar=sqrt(xx^2+yy^2+zz^2);
   sita=atan(yy/xx);
   yeta=atan(zz/(sqrt(xx^2+yy^2)));
A=[ar sita yeta]';
   shuchu_real=A+shuchu_error;
   x=x_previous(1);
     y=x_previous(4);
     z=x_previous(7);
     r=sqrt(x^2+y^2+z^2);
     ar_previous=sqrt(x^2+y^2+z^2);
   sita_previous=atan(y/x);
   yeta_previous=atan(z/(sqrt(x^2+y^2)));
   shuchu_previous=[ar sita yeta]';
     
     %x=observation(1)*cos(observation(2))*cos(observation(3));
     %y=observation(1)*sin(observation(2))*cos(observation(3));
     %z=observation(1)*sin(observation(3));
     
     h=[x/r 0 0 y/r 0 0 z/r 0 0;
        -y/(x^2+y^2) 0 0 x/(x^2+y^2) 0 0 0 0 0;
         -x*z/(r^2*sqrt(x^2+y^2)) 0 0 -y*z/(r^2*sqrt(x^2+y^2)) 0 0 sqrt(x^2+y^2)/r^2 0 0];
     p_previous=l*p00*l'+w_error;
     s=h*p_previous*h'+r_error;
     k=p_previous*h'*inv(s);
     
     x_estimate=x_previous+k*(shuchu_real-shuchu_previous);
     x_estimate_store(:,time)=x_estimate;
     c=[1 1 1 1 1 1 1 1 1];
     I=diag(c);
     p_estimate=(I-k*h)*p_previous;
     x00=x_estimate;
     p00=p_estimate;
   end;
 figure;
 plot(x_previous,'r');
 hold on;
 plot(x_estimate,'m');
 hold off;