基于卡尔曼滤波的运动目标检测_matlab代码_kalman tracking

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