www.pudn.com > kalman.zip > Kalman.m, change:2014-11-06,size:1120b


B=[];
A=importdata('row3.csv');     %导入数据
A=A+10;
i=1;
while i<=length(A)            
    if A(i)>=40       
        A(i)=[];
    else
        i=i+1;
    end
end
x=1:5548;
B=A-5;                         

X=[];    %初始化
J=[];
C=1.3;
X(1)=0;
i=1;
J(i)=B(i)/43;
Je=[];
N=5548;
w=randn(1,N); %产生一个1×N的行向量,第一个数为0,w为过程噪声(其和后边的v在卡尔曼理论里均为高斯白噪声)
w(1)=0;
w=w*0.01;
Q=var(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维,方差与协方差相同) 

v=randn(1,N);%测量噪声
v=v*0.01;
R=var(v);
% plot(w)
while i<length(B)            %卡尔曼滤波
    X(i+1)=0.8*X(i)+0.5*(J(i)-X(i))+w(i);
    i=i+1;
    J(i)=B(i)/43;
    Je(i)=C*X(i)+v(i);
end

plot(x,Je,'LineWidth',2);
hold on;
plot(x,J,'g');
axis([0,5400,0,1]);
set(gca, 'XTick', [0:900:5400]);%X坐标轴刻度数据点位置
set(gca,'Xticklabel',[0:20:120]);
set(gca, 'YTick', [0:0.2:1]) %X坐标轴刻度数据点位置
set(gca,'Yticklabel',[0:0.2:1]);
xlabel('时刻/秒','fontsize',16);
ylabel('安全态势','fontsize',16);
set(gca,'gridlinestyle',':','linewidth',3);
h=legend('预测值','实际值',2);
set(h,'FontSize',20);
set(gca,'ygrid','on');