kalman.m

本源码是一个用Matlab7.1实现的Kalman的源代码

function [x2_k,Pkk]=kalman(r_estimate,estimate,x2_k,z2_k,Pkk,L)
%i表示第i次滤波
Tf=1.0;%滤波周期
%Fk_1=f(r_estimate(2:(2*L+1),1),r_estimate(1,1),L);%f为双曲正切函数
%Fk=f(estimate(2:(2*L+1),1),r_estimate(1,1),L);%f为双曲正切函数
%F=(Fk-Fk_1)';
%H=F;%l维行向量
F=f(estimate(2:(2*L+1),1),estimate(1,1),L);
H=F';
Fkk_1=zeros(L,L);
for ii=1:L
    Fkk_1(ii,ii)=1.0;
end
Qc=zeros(L,L);%l维
for ii=1:L
Qc(ii,ii)=0.00001;
end

Rv=zeros(1,1);%n维
Rv(1,1)=1*0.08;

Xkk=x2_k;%2*l维
Arg.type = 'gaussian';
Arg.dim=1;
Arg.mu  = zeros(Arg.dim,1);
Arg.cov_type = 'full';
Arg.cov=zeros(Arg.dim,Arg.dim);
Arg.cov(1,1)=0.00001;
pNoise = gennoiseds(Arg);            % process noise : zero mean white Gaussian noise, cov = (1e-3)^2(dynamics) and (1e-4)^2 (tone frequency, very stable)
ppnoise = feval(pNoise.sample,pNoise, 1);

Zk=z2_k+ppnoise;
%Qk_1是Qc的离散化
m_tmp5=Fkk_1';  
m_tmp1=Qc*m_tmp5;
%Qk_1=Qc+Fkk_1*m_tmp1
m_temp4=Fkk_1*m_tmp1;
Qk_1=Qc+m_temp4;

	for ii=1:L
    for j=1:L
	    Qk_1(ii,j)=Qk_1(ii,j)*Tf/2.0;%Qk_1=(Qc+Fkk_1*Qc*Fkk_1')*Tf/2
    end
    end
%Pkk_1

	%m_tmp1=Pkk*Fkk_1'
    m_tmp1=Pkk*m_tmp5;
	%Pkk_1=Fkk_1*m_tmp1+Qk_1
    m_temp4=Fkk_1*m_tmp1;
    Pkk_1=m_temp4+Qk_1;
%Kk
	%m_tmp2=Pkk_1*H'
    m_tmp6=H';
    m_tmp2=Pkk_1*m_tmp6;
    
	%m_tmp3=H*m_tmp2+Rv
    m_tmp8=H*m_tmp2;
    m_tmp3=Rv+m_tmp8;
    %m_tmp3=inv(m_tmp3);
	m_tmp3=1.0/m_tmp3;
    %Kk=m_tmp2*m_tmp3
    KK=m_tmp2*m_tmp3;
    
    %for (i=0;i<2*l;i++) Xkk[i]=0.0;
  	%Xkk_1=Fkk_1*Xkk
    Xkk_1=Fkk_1*Xkk;
    
	%v_tmp1=H*Xkk_1
    v_tmp1=H*Xkk_1;
    for ii=1:1
	    v_tmp1(ii,1)=Zk(ii,1)-v_tmp1(ii,1);
    end
	%XKK=XKK_1+KK*v_tmp1
    m_tmp7=KK*v_tmp1;
    Xkk=Xkk_1+m_tmp7;

	%Pkk
	%m_tmp1=I-KK*H
    m_tmp1=KK*H;
    
	for ii=1:L
    for j=1:L
	    m_tmp1(ii,j)=-m_tmp1(ii,j);
    end
    end
	for ii=1:L
	    m_tmp1(ii,ii)=m_tmp1(ii,ii)+1;
    end
	%Pkk=m_tmp1*Pkk_1
    Pkk=m_tmp1*Pkk_1;
    x2_k=Xkk;