kalman1.m

基于matlab的几个kalman滤波以及多传感器融合和wk算法的程序

%卡尔曼滤波  匀速直线运动
%信号模型：X(k+1)=H*X(k)+L*W(k)   
%观测模型：Z(k)=C*X(k)+V(k)

clear;
clc;

IdealTrack=zeros(2,200);
for k=1:200
    IdealTrack(1,k)=2000;
    IdealTrack(2,k)=10000-30*k;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Noise=100*randn(2,200);
MeasureTrack=zeros(2,200);
MeasureTrack=IdealTrack+Noise;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FilterError=zeros(100,200);
FilterState=zeros(4,1);
ForcastState=zeros(4,1);
for n=1:100                  %%蒙特卡罗模拟次数为100
    H=[1 2 0 0
         0 1 0 0 
         0 0 1 2
         0 0 0 1];
    L=[2 0 
         2 0 
         0 2 
         0 2];
    Q=[100 0
         0 100];
    R=[10000 0
         0 10000];
    C=[1 0 0 0
         0 0 1 0];
     FilterTrack=zeros(2,200);
    
    FilterTrack(:,1)=MeasureTrack(:,1);
    FilterTrack(:,2)=MeasureTrack(:,2);
    x2=MeasureTrack(1,2);
    y2=MeasureTrack(2,2);
    Vx2=(MeasureTrack(1,2)-MeasureTrack(1,1))/2;
    Vy2=(MeasureTrack(2,2)-MeasureTrack(2,1))/2;  

   FilterState=[x2 Vx2 y2 Vy2]';
   Pfilter=[10000 5000 0 0
       10000 5000 0 0
       0 0 10000 5000                               %%滤波的初值选定
       0 0 5000 5000];                        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for k=3:200
        ForcastState=H*FilterState;                                         %%预测状态
        Pforcast=H*Pfilter*H';                                              %%预测误差协方差
        Kgain=Pforcast*C'*inv(C*Pforcast*C'+R);                              %%增益
        FilterState=ForcastState+Kgain*(MeasureTrack(:,k)-C*ForcastState);  %%滤波状态
        Pfilter=(eye(4)-Kgain*C)*Pforcast;                                  %%滤波误差协方差
        FilterTrack(:,k)=C*FilterState;   
        FilterError(n,k)=sqrt((FilterTrack(1,k)-IdealTrack(1,k))^2+(FilterTrack(2,k)-IdealTrack(2,k))^2);
   end
   for k=1:2
        FilterError(n,k)=sqrt((FilterTrack(1,k)-IdealTrack(1,k))^2+(FilterTrack(2,k)-IdealTrack(2,k))^2);
   end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MeanError=zeros(1,200);
MeanError=sum(FilterError,1)/100;
StandError=zeros(200);
StandardDeviation=zeros(200);
for k=1:200
    for n=1:100
        FFilterError(n,k)=FilterError(n,k)^2;
    end
    StandError=sum(FFilterError,1)/100;
   StandardDeviation(k)=sqrt(abs(StandError(k)-MeanError(k)^2));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(2,2,1);
plot(IdealTrack(1,:),IdealTrack(2,:),'b'); 
xlabel('理想航迹和观测模型');
axis([1000,3000,2000,12000]);
hold on;
plot(MeasureTrack(1,:),MeasureTrack(2,:),'r');    %%绘制理想航迹和观测模型

subplot(2,2,2);
plot(IdealTrack(1,:),IdealTrack(2,:),'b'); 
axis([1000,3000,2000,12000]);
hold on;
plot(FilterTrack(1,:),FilterTrack(2,:),'k');
xlabel('理想航迹和滤波结果');

subplot(2,2,3);
plot(MeanError);
xlabel('误差均方差');

subplot(2,2,4);
plot(StandardDeviation);
xlabel('标准差');