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
