trail_kal.m

利用matlab实现的简单的基于卡尔曼滤波的跟踪算法

%incremental algorithm
%C--balance parameter
%z--the weight vector of SVM
%u--the output of SVM
clc;
clear;
N=100;
e=0.1:10/100:10;
xe=sin(e);
y=2*xe;
cita=2;
d=1;
C=zeros(N,N+1);
z=zeros(N+1,N+1);
z(:,1)=0.01*ones(N+1,1);
% z(:,1)=randn(N+1,1)/250;
R=1.001*eye(N+1,N+1);% covariant
ee=zeros(N,1);
Q=1.1;           % covariant
I=eye(N+1,N+1);
P=1000000.01*eye(N+1,N+1);
yy=zeros(N,1);
for i=1:N
    uk(i)=d*exp(-cita*((xe(i)-xe(1))*(xe(i)-xe(1))));
end
C(1,:)=[1,uk];
G=(P'+R)*C(1,:)'*inv(C(1,:)*P*C(1,:)'+Q);
P=(I-G*C(1,:))*(P'+R);
for j=1:N-1
     ee(j)=yy(j)-y(j);
     for i=1:N
         K(i)=d*exp(-cita*((xe(i)-xe(j))^2));%+(ee(i)-ee(j))^2));
     end
%     R=0.9*P; 
    C(j+1,:)=[1,K];
    G=(P'+R)*C(j,:)'*inv(Q+C(j,:)*(P'+R)*C(j,:)');        %gain matrix
    z(:,j+1)=z(:,j)+G*(y(j)-C(j,:)*z(:,j));
    P=(I-G*C(j,:))*(P'+R);
    yy(j+1)=C(j+1,:)*z(:,j+1);
end
sume=ee'*ee
figure;
plot(yy,'r');
hold on
plot(y,'k');