卡尔曼

clear,clc % 计算背景图像 Imzero = zeros(240,320,3); for i = 1:5 Im{i} = double(imread(['DATA/',int2str(i),'.jpg'])); Imzero = Im{i}+Imzero; end Imback = Imzero/5; [MR,MC,Dim] = size(Imback); % Kalman滤波器初始化 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; Bu = [0,0,0,g]'; kfinit=0; x=zeros(100,4); % 循环遍历所有图像 for i = 1 : 60 % 导入图像 Im = (imread(['DATA/',int2str(i), '.jpg'])); imshow(Im) imshow(Im) Imwork = double(Im); %提取球的质心坐标及半径 [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i); if flag==0 continue end %用绿色标出球实际运动的位置 hold on 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 % Kalman器更新 if kfinit==0 xp = [MC/2,MR/2,0,0]' else xp=A*x(i-1,:)' + Bu 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 pause(0.3) end % 画出球横纵坐标的位置 figure plot(cc,'r*') hold on plot(cr,'g*') %噪声估计 posn = [cc(55:60)',cr(55:60)']; mp = mean(posn); diffp = posn - ones(6,1)*mp; Rnew = (diffp'*diffp)/5;

 

 

%Kalman滤波 clear N=800; w(1)=0; %系统预测的随机白噪声 w=randn(1,N) x(1)=0; a=1; for k=2:N; %系统的预测值 x(k)=a*x(k-1)+w(k-1); end %测量值的随机白噪声 V=randn(1,N); q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2; %测量值 Y=c*x+V; p(1)=0; s(1)=0; for t=2:N; %前一时刻X的协方差系数 p1(t)=a.^2*p(t-1)+Rww; %Kalman增益 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); %经过滤波后的信号 s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); %t状态下x(t|t)的协方差系数 p(t)=p1(t)-c*b(t)*p1(t); end subplot(131) plot(x) title('系统的预测值') subplot(132) plot(Y) title('测量值') subplot(133) plot(s) title('滤波后的信号')

 

function [cc,cr,radius,flag]=extractball(Imwork,Imback,index) % 功能：提取图像中最大斑点的质心坐标及半径 % 输入：Imwork-输入的当前帧的图像；Imback-输入的背景图像；index-帧序列图像序号 % 输出：cc-质心行坐标；cr-质心列坐标；radius-斑点区域半径；flag-标志 cc = 0; cr = 0; radius = 0; flag = 0; [MR,MC,Dim] = size(Imback); % 将输入图像与背景图像相减，获得差异最大的区域 fore = zeros(MR,MC); fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) ... | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) ... | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10); foremm = bwmorph(fore,'erode',2); % 运用数学形态学去除微小的噪声 % 选择大的斑点对其周围进行标记 labeled = bwlabel(foremm,4); stats = regionprops(labeled,['basic']); [N,W] = size(stats); if N < 1 return end % 如果大的斑点的数量大于1，则用冒泡法进行排序 id = zeros(N); for i = 1 : N id(i) = i; end for i = 1 : N-1 for j = i+1 : N if stats(i).Area < stats(j).Area tmp = stats(i); stats(i) = stats(j); stats(j) = tmp; tmp = id(i); id(i) = id(j); id(j) = tmp; end end end % 确定并选取一个大的区域 if stats(1).Area < 100 return end selected = (labeled==id(1)); % 获得最大斑点区域的圆心及半径，并将标志置为1 centroid = stats(1).Centroid; radius = sqrt(stats(1).Area/pi); cc = centroid(1); cr = centroid(2); flag = 1; return

转载于:https://www.cnblogs.com/dverdon/p/5300435.html