cvfilter.m

这个个程序是利用卡尔曼滤波实现人体关节点的跟踪问题

%%%%%%%%%%%%%%%%%  基于匀速模型的滤波函数  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%系统模型  x(k+1)=Fk*x(k)+C*W(k)

   function cv=cvfilter(T,N,d,Z)                
  
 F=[1 T  0 0 
   0 1  0 0
   0 0  1 T 
   0 0  0 1 ];%状态转移矩阵
     

C=[T^2/2  0 ;
    T     0  ;
    0     T^2/2 ;
    0     T];%噪声转移矩阵


Qi=[0.03^2,0; 0,0.08^2];%%process covariance and measurement covariance

H=[1 0 0 0 
   0 0 1 0 ]; %量测转移矩阵
Ri=d^2*eye(2);%量测方差
I=eye(4);
%T=1;%采样间隔
%$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$%%%%%%%%%%%%%%%%%%%%%%%%%

%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&%%%%%%%%%%%%%%%%%%%%%%%%%%
xx1=[30000 -172  30000 -246  ]';%初始位置

PP1=200^2*eye(4);%状态预测协方差
cv=[];
%滤波递推.
           
     for i=1:N
     x1=F*xx1;%计算一步预测估计值
     P11=F*PP1*F'+C*Qi*C';%计算预测滤波协方差矩阵
     K1=P11*H'*inv(H*P11*H'+Ri);%计算增益矩阵
     %S1=H*P11*H'+Ri;%滤波参差
     PP1=(I-K1*H)*P11;%预测协方差
     xx1=x1+K1*(Z(:,i)-H*x1);%计算估计值
     cv=[cv xx1];%输出
     end