clear,clc
% compute the background image
Imzero = zeros(240,360,3);
video=aviread('SampleVideo.avi');
Im = double(cat(4,video(1:2:end).cdata))/255;
clear video
% Convert to RGB to GRAY SCALE image.
nFrames = size(Im,4);
for i = 1:5
%Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
Imzero = Im(:,:,:,i)+Imzero;
end
Imback = Imzero/5*255;
[MR,MC,Dim] = size(Imback);
% Kalman filter initialization
R=[[0.2845,0.0045]',[0.0045,0.0455]']; %观测噪声协方差矩阵
H=[[1,0]',[0,1]',[0,0]',[0,0]']; %观测转移矩阵
Q=0.01*eye(4); %状态噪声协方差矩阵
P = 100*eye(4); %误差协方差
dt=1;
A=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]']; %状态转移矩阵
%g = 6; % pixels^2/time step
%Bu = [0,0,0,g]';
kfinit=0; %初始指针
x=zeros(100,4); %状态矩阵
% loop over all images
for i = 1 : nFrames
% load image
imshow(Im(:,:,:,i))
hold on
%imshow(Im)
Imwork = Im(:,:,:,i)*255;
%extract ball
[x2(i),y2(i),width_x(i),width_y(i),cc(i),cr(i),flag] = extract(Imwork,Imback,i);
if flag==0
continue
end
%for c = -1*radius: radius/20 : 1*radius
%r = sqrt(radius^2-c^2);
%plot(cc(i)+c,cr(i)+r,'g.')
%plot(cc(i)+c,cr(i)-r,'g.')
%end
if (width_x(i)~=0) & (width_y(i)~=0)
rectangle('Position',[x2(i) y2(i) width_x(i) width_y(i)],'EdgeColor','r');
end
if (width_x(i)~=0) & (width_y(i)~=0)
plot(cc(i),cr(i), 'r+');
end
hwidth_x(i)=cc(i)-x2(i);
hwidth_y(i)=cr(i)-y2(i);
% Kalman update
i
if kfinit==0
xp = [cc(i),cr(i),0,0]' %预测状态矩阵
else
xp=A*x(i-1,:)'
end
kfinit=1;
PP = A*P*A' + Q %预测误差协方差
K = PP*H'*inv(H*PP*H'+R)
x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
x(i,:)
[cc(i),cr(i)]
P = (eye(4)-K*H)*PP
hold on
%for c = -1*radius: radius/20 : 1*radius
%r = sqrt(radius^2-c^2);
%plot(x(i,1)+c,x(i,2)+r,'r.')
%plot(x(i,1)+c,x(i,2)-r,'r.')
%end
if (width_x(i)~=0) & (width_y(i)~=0)
rectangle('Position',[(x(i,1)-hwidth_x(i)) (x(i,2)-hwidth_y(i)) width_x(i) width_y(i)],'EdgeColor','g');
end
% pause(0.3)
x1(i)=x(i,1)-cc(i)
y1(i)=x(i,2)-cr(i)
hold on
if (width_x(i)~=0) & (width_y(i)~=0)
plot(x(i,1),x(i,2), 'g+');
end
drawnow
end
% show positions
figure
plot(x1,'r*')
hold on
plot(y1,'g*')
%end
%estimate image noise (R) from stationary ball
posn = [cc(55:60)',cr(55:60)'];
mp = mean(posn);
diffp = posn - ones(6,1)*mp;
Rnew = (diffp'*diffp)/5