卡爾曼

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