yht1.m

卡尔曼滤波器的主要功能就是目标跟踪

%===============================
%Recursive least square
%===============================
clear all;
tic
axis=linspace(1,200,200);
x_ErrorSum=zeros(1,200);
y_ErrorSum=zeros(1,200);
x_ErrorSquare=zeros(1,200);
y_ErrorSquare=zeros(1,200);
    %以下产生真实值
    for k=1:200
        x_real(k)=2000;
        y_real(k)=10000-15*2*k;
    end
for i=1:50  %-------根据蒙特卡洛思想，做50次仿真
    %以下产生观测值
    for k=1:200
        x_observe(k)=2000+normrnd(0,100);
        y_observe(k)=10000-15*2*k+normrnd(0,100);
    end
    %以下用前两个观测值产生初始值
    x_estimate(1)=x_observe(1);
    y_estimate(1)=y_observe(1);
    x_estimate(2)=x_observe(2);
    y_estimate(2)=y_observe(2);
    begin_vector=[1 2;1 4]\[y_observe(1) y_observe(2)]';
    P=inv([1 2;1 4]'*inv(10000*eye(2))*[1 2;1 4]);
    %以下执行循环算法
    for k=3:200
        K=P*[1 2*k]'/([1 2*k]*P*[1 2*k]'+10000);
        begin_vector=begin_vector+K*(y_observe(k)-[1 2*k]*begin_vector);
        P=(eye(2)-K*[1 2*k])*P;
        y_estimate(k)=[1 2*k]*begin_vector;
        x_estimate(k)=(x_estimate(k-1)*(k-1)+x_observe(k))/k;
    end
    %以下纪录每一个时刻的估计误差和聚估计误差的平方
    for k=1:200
        x_ErrorSum(k)=x_ErrorSum(k)+(x_real(k)-x_estimate(k));
        y_ErrorSum(k)=y_ErrorSum(k)+(y_real(k)-y_estimate(k));
        x_ErrorSquare(k)=x_ErrorSquare(k)+(x_real(k)-x_estimate(k))^2;
        y_ErrorSquare(k)=y_ErrorSquare(k)+(y_real(k)-y_estimate(k))^2;
    end
end
%以下计算误差均值向量和标准差向量
x_ErrorMean=x_ErrorSum./50;
y_ErrorMean=y_ErrorSum./50;
x_ErrorSigma=(x_ErrorSquare./50-x_ErrorMean.^2).^0.5;
y_ErrorSigma=(y_ErrorSquare./50-y_ErrorMean.^2).^0.5;
%以下输出显示
figure(1);
plot(x_real,y_real,':',x_observe,y_observe,'*',x_estimate,y_estimate,'r-');
title('目标跟踪轨迹');
figure(2);
subplot(2,1,1);
plot(axis,x_ErrorMean,'b'),grid on;
title('x方向误差的均值');
subplot(2,1,2);
plot(axis,y_ErrorMean,'b'),grid on;
title('y方向误差的均值');
figure(3);
subplot(2,1,1);
plot(axis,x_ErrorSigma,'b-'),grid on;
title('x方向误差的标准差');
subplot(2,1,2);
plot(axis,y_ErrorSigma,'b'),grid on;
title('y方向误差的标准差');
toc