计算机视觉
图像处理

离散随机线性系统的卡尔曼滤波器基本原理及实现

介绍Kalman滤波器理论知识,并给出一个演示的例子。由于Kalman滤波在目标跟踪时,需要不断获取观测向量,所以没法单独使用。如果时间充裕,下一篇博文将会做基于MeanShift + Kalman的目标跟踪。这次的主要结构:

1.       卡尔曼滤波基本原理

2.       卡尔曼滤波器算法

3.       演示例子【来自课本:C语言常用算法程序集(第二版)】+网上广为流传的自由落体小球跟踪matlab

 

一、离散随机线性系统的卡尔曼滤波器基本原理

卡尔曼 滤波方法是一种递推的状态空间方法,其基本特征是利用状态方程描述状态的转移过程,使用观测方程获得对状态的观测信息。它只用状态的前一个估计值和最近一 个观察值就可以在线性无偏最小方差估计准则下对当前状态作出最优估计。【对于时变系统来说,状态是关于时间的变量】

【这里,我特别强调一下观察值,必须更新,否则后验估计将不准确(比如突然改变运动方向)。Opencv的跟踪鼠标例子、和网上流传的小球自由落体跟踪,它的观测值始终在更新】

下面,首先给出描述离散时变线性系统的状态方程和观测方程:

 

其中,xkk时刻系统特征的状态向量,zk为观测向量,A为状态由k-1时刻到k时刻的转移矩阵,uk-1k-1时刻可选输入控制矩阵,B为可选输入控制矩阵的增益矩阵【这边可选的就不选了】,H表示状态向量xk对观测向量zk的增益,通常为常数矩阵。wkvk分别表示k时刻系统输入随机噪声向量和观测噪声向量,通常为常数矩阵。

卡尔曼滤波器要解决的基本问题就是如何在k时刻已知观测变量zk的情况下求解状态向量xk线性最小方差估计。卡尔曼滤波器将状态视为抽象状态空间中的点,进而利用在Hilbert空间中定义的投影理论解决状态的最优估计问题。【数学推导略】

 

 

 

二、离散卡尔曼滤波器算法

卡尔曼滤波器分为两个部分:状态预测方程(状态预估)和状态修正方程(观测更新)。状态预测方程负责及时向前推算当前状态变量和误差协方差的估计值,以便 为下一时间状态构造先验估计。状态修正方程负责反馈,即将先验估计和新的观测变量结合以构造改进的后验估计。

下面对时间离散点上的采样数据进行卡尔曼滤波的公式进行完整梳理。

n为线性动态系统和m维线性观测系统由下列方程描述:

xkn为向量,表示系统在第k时刻的状态

A是一个n*n阶矩阵,成为系统的状态转移矩阵,它反映了系统从第k-1个采样时刻的状态到第k个采样时刻的状态变换

wk-1是一个n为向量,表示在第k时刻作用于系统的随机干扰,称为模型噪声,为简单起见,一般假设wk为高斯白噪声序列,具有已知的零均值和协方差阵Qk

zkm维观测向量

Hm*n阶观测矩阵,表示了从状态xk到观测量zk的转换

vkm维观测噪声,同样假设vk为高斯白噪声序列,具有已知的零均值和协方差阵Rk

 

经过推导【略】,可得如下滤波的递推公式:

Qkn*n阶的模型噪声wk的协方差阵

Rkm*m阶的观测噪声vk的协方差阵

Kkn*m阶增益矩阵

。为n维向量,第k时刻经过滤波后的估值

Pkn*n阶的估计误差协方差阵

根据上述公式,可以从x0=E{x0},给定的P0出发,利用已知的矩阵QkRkHA以及k时刻的观测值zk,递推地算出每个时刻状态的估值

如果线性系统是定常的,则AH都是常数矩阵;如果模型噪声wk和观测噪声vk都是平稳随机序列,则QkRk为常数矩阵。在这种情况下,常增益的离散卡尔曼滤波是渐近稳定的。

下面给出卡尔曼滤波器工作原理图:

三、演示例子

实验一:

    来自《C常用算法程序集第二版》 Chapter 11.5

    我觉得是一个跟踪变速运动质点的粒子,结果也没什么意思,分别显示了距离、速度、加速度的估计量。【这个例子帮助了解卡尔曼滤波的流程】

    设信号源运动方程为:s(t) = 5 – 2t + 3t2+v(t),其中v(t)是一个均值为0,方差为0.25的高斯白噪声。状态向量为xk=(s,s’,s’’)T,并取初值x0=(0,0,0)T

状态转移矩阵为:

     观测矩阵H =(1,0,0)

     观测向量为信号源运动方程产生

     观测噪声协方差矩阵 R=0.25

     初始估计误差协方差矩阵:

 

主函数:

// Kalman.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"

void keklm5(int n,double y[]);
int klman(int n,int m,int k,double *f,double *q,double *r,double *h,double *y,double *x,double *p,double *g);

int _tmain(int argc, _TCHAR* argv[])
{
	int i,j,js;
	//s为真值,y为迭加有高斯白噪声的采样值
	double p[3][3],x[200][3],y[200][1],g[3][1],t,s;
	static double f[3][3]={{1.0,0.05,0.00125},{0.0,1.0,0.05},{0.0,0.0,1.0}};
	static double q[3][3]={{0.25,0.0,0.0},{0.0,0.25,0.0},{0.0,0.0,0.25}};
	static double r[1][1]={0.25};
	static double h[1][3]={1.0,0.0,0.0};

	for (i=0; i<=2; i++)
		for (j=0; j<=2; j++) 
			p[i][j]=0.0;

	for (i=0; i<=199; i++)
		for (j=0; j<=2; j++) 
			x[i][j]=0.0;

	keklm5(200,&y[0][0]);

	for (i=0; i<=199; i++)
	{ 
		t=0.05*i;
		y[i][0]=5.0-2.0*t+3.0*t*t+y[i][0];
	}
	
	js = klman(3,1,200,&f[0][0],&q[0][0],&r[0][0],&h[0][0],&y[0][0],&x[0][0],&p[0][0],&g[0][0]);
	
	if (js!=0)
	{ 
		printf("\n");
		printf("   t       s            y            ");
		printf("x(0)         x[1]         x(2)   \n");
		
		for (i=0; i<=199; i=i+5)
		{
			t=0.05*i; 
			s=5.0-2.0*t+3.0*t*t;
			printf("%6.2f  %e  %e  %e  %e  %e\n",t,s,y[i],x[i][0],x[i][1],x[i][2]);
		}
		printf("\n");
	}

	return 0;
}

//产生均值为0,方差为0.25的高斯白噪声序列
void keklm5(int n,double *y)
{
	int i,j,m;
	double s,w,v,r,t;

	s=65536.0; 
	w=2053.0;
	v=13849.0; 
	r=0.0;

	for (i=0; i<=n-1; i++)
	{ 
		t=0.0;
		for (j=0; j<=11; j++)
		{
			r=w*r+v; 
			m=r/s;
			r=r-m*s; 
			t=t+r/s;
		}
		y[i]=0.5*(t-6.0);
	}
	return;
}

卡尔曼滤波函数:

函数参数说明:

#include "stdlib.h"
#include "stdio.h"

extern int brinv(double *e,int m);

int klman(int n,int m,int k,double *f,double *q,double *r,double *h,double *y,double *x,double *p,double *g)
{
	int i,j,kk,ii,l,jj,js;
	double *e,*a,*b;

	e= (double *)malloc(m*m*sizeof(double));
	l=m;
	if (l

矩阵求逆函数:

#include "stdlib.h"
#include "math.h"
#include "stdio.h"

//矩阵求逆
int brinv(double *a,int n)
{
	int *is,*js,i,j,k,l,u,v;
	double d,p;
	is= (int *)malloc(n*sizeof(int));
	js= (int *)malloc(n*sizeof(int));

	for (k=0; k<=n-1; k++)
	{
		d=0.0;
		for (i=k; i<=n-1; i++){
			for (j=k; j<=n-1; j++)
			{
				l=i*n+j; p=fabs(a[l]);
				if (p>d) { d=p; is[k]=i; js[k]=j;}
			}
		}

		if (d+1.0==1.0)
		{
			free(is); free(js); printf("err**not inv\n");
			return(0);
		}

		if (is[k]!=k){
			for (j=0; j<=n-1; j++)
			{
				u=k*n+j; v=is[k]*n+j;
				p=a[u]; a[u]=a[v]; a[v]=p;
			}
		}

		if (js[k]!=k){
			for (i=0; i<=n-1; i++)
			{
				u=i*n+k; v=i*n+js[k];
				p=a[u]; a[u]=a[v]; a[v]=p;
			}
		}

		l=k*n+k;
		a[l]=1.0/a[l];

		for (j=0; j<=n-1; j++){
			if (j!=k)
			{
				u=k*n+j; a[u]=a[u]*a[l];
			}
		}
		for (i=0; i<=n-1; i++){
			if (i!=k)
				for (j=0; j<=n-1; j++)
					if (j!=k)
					{ 
						u=i*n+j;
						a[u]=a[u]-a[i*n+k]*a[k*n+j];
					}
		}
		for (i=0; i<=n-1; i++){
			if (i!=k)
			{
				u=i*n+k; a[u]=-a[u]*a[l];
			}
		}
	}

	for (k=n-1; k>=0; k--)
	{
		if (js[k]!=k){
			for (j=0; j<=n-1; j++)
			{
				u=k*n+j; v=js[k]*n+j;
				p=a[u]; a[u]=a[v]; a[v]=p;
			}
		}
		if (is[k]!=k){
			for (i=0; i<=n-1; i++)
			{
				u=i*n+k; v=i*n+is[k];
				p=a[u]; a[u]=a[v]; a[v]=p;
			}
		}
	}

	free(is); free(js);
	return(1);
}

实验结果:

t                   s                            y                             x(0)                        x(1)                       x(2)
0.00  5.000000e+000  7.084644e-318  0.000000e+000  0.000000e+000  0.000000e+000
0.25  4.687500e+000  -4.991061e-227  2.592479e-251  -2.138112e+244  5.259612e-315
0.50  4.750000e+000  2.295683e-249  -5.479083e+050  1.311797e-111  5.260019e-315
0.75  5.187500e+000  5.246780e+304  5.115994e+116  -2.770793e+052  5.273744e-315
1.00  6.000000e+000  1.044097e-247  -4.339277e+276  2.386454e+305  5.285550e-315
1.25  7.187500e+000  2.832342e+043  -3.291637e-036  2.694055e+067  5.285385e-315
1.50  8.750000e+000  6.025928e+301  -1.223782e-073  1.264464e+141  5.297514e-315
1.75  1.068750e+001  5.766441e-279  -5.825451e-182  1.670409e+213  5.300126e-315
2.00  1.300000e+001  -3.259575e+058  -4.987811e-194  4.040332e+216  5.304279e-315
2.25  1.568750e+001  -4.434782e-141  2.673119e+000  -7.093193e-002  5.306660e-315
2.50  1.875000e+001  4.643765e+253  -2.036111e-257  3.166038e-301  5.307847e-315
2.75  2.218750e+001  1.585343e+264  -5.172634e+256  2.839756e-003  5.308454e-315
3.00  2.600000e+001  -5.934871e-140  -2.523842e+134  9.679633e-224  5.310177e-315
3.25  3.018750e+001  1.030377e+077  -5.176784e-124  5.691396e-034  5.310969e-315
3.50  3.475000e+001  -3.030478e+090  -3.037085e+136  -1.664247e+210  5.311456e-315
3.75  3.968750e+001  -1.506812e+082  3.941046e+263  1.069353e+060  5.312293e-315
4.00  4.500000e+001  5.174961e-088  7.976089e-075  -7.936906e+282  5.311919e-315
4.25  5.068750e+001  -6.889139e+246  -4.400495e+166  -3.520872e+090  5.312644e-315
4.50  5.675000e+001  1.119620e-157  -5.682003e+133  -5.907501e-163  5.313093e-315
4.75  6.318750e+001  4.974372e+150  -9.557502e+125  5.398731e+106  5.312929e-315
5.00  7.000000e+001  2.342533e-137  -8.646144e+094  -1.845627e+171  5.312839e-315
5.25  7.718750e+001  8.294805e+248  9.873377e-079  2.098667e+240  5.313285e-315
5.50  8.475000e+001  -1.804055e+197  8.471683e+137  -1.923938e+222  5.313494e-315
5.75  9.268750e+001  -1.835060e-292  2.856969e+170  -2.933085e-158  5.312953e-315
6.00  1.010000e+002  1.635051e+082  3.547906e+205  1.240273e+009  5.313658e-315
6.25  1.096875e+002  3.055828e+070  1.489807e-142  -1.860817e-246  5.313093e-315
6.50  1.187500e+002  -1.810083e-287  6.504162e-186  1.415815e+069  5.313024e-315
6.75  1.281875e+002  5.524521e+258  4.463901e-038  8.980017e+047  5.313017e-315
7.00  1.380000e+002  7.212461e-023  3.698627e+050  -4.210630e+274  5.313149e-315
7.25  1.481875e+002  4.735847e-042  -1.874845e-294  5.067841e+064  5.313126e-315
7.50  1.587500e+002  -2.980172e+234  1.374812e-210  1.088393e-308  5.312923e-315
7.75  1.696875e+002  -2.697914e-019  -5.252701e+057  -1.200521e+250  5.312829e-315
8.00  1.810000e+002  -4.464229e-223  -6.472546e+053  -1.133199e-062  5.313024e-315
8.25  1.926875e+002  2.682346e+304  9.547456e-048  1.331390e-082  5.313036e-315
8.50  2.047500e+002  -4.990961e+238  -5.366085e-282  -2.288856e+046  5.313031e-315
8.75  2.171875e+002  -6.431324e+035  -1.378076e+305  5.655992e-070  5.313412e-315
9.00  2.300000e+002  -8.586463e-275  9.381069e+297  -4.623112e-203  5.312526e-315
9.25  2.431875e+002  -2.089942e+254  9.490370e+001  2.385556e-117  5.312642e-315
9.50  2.567500e+002  3.565267e+214  -1.587972e+186  8.876997e+074  5.312320e-315
9.75  2.706875e+002  1.149508e-252  -3.874825e-077  -3.999077e-283  5.312839e-315

 

实验二:小球自由落体

说明:读取图片序列,首先进行背景建模【取前5帧平均值】,然后将前景区域进行腐蚀膨胀操作,作为观测向量Zk,其余初始化之类的直接读代码。

 

提取小球部分:

% extracts the center (cc,cr) and radius of the largest blob
function [cc,cr,radius,flag]=extractball(Imwork,Imback,index)%,fig1,fig2,fig3,fig15,index)
  
  cc = 0;
  cr = 0;
  radius = 0;
  flag = 0;
  [MR,MC,Dim] = size(Imback);

  % subtract background & select pixels with a big difference
  fore = zeros(MR,MC);          %image subtracktion
  fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) ...
     | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) ...
     | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10);  

  % Morphology Operation  erode to remove small noise
  foremm = bwmorph(fore,'erode',2); %2 time

  % select largest object
  labeled = bwlabel(foremm,4);
  stats = regionprops(labeled,['basic']);%basic mohem nist
  [N,W] = size(stats);
  if N < 1
    return   
  end

  % do bubble sort (large to small) on regions in case there are more than 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

  % make sure that there is at least 1 big region
  if stats(1).Area < 100 
    return
  end
  selected = (labeled==id(1));

  % get center of mass and radius of largest
  centroid = stats(1).Centroid;
  radius = sqrt(stats(1).Area/pi);
  cc = centroid(1);
  cr = centroid(2);
  flag = 1;
  return

主要处理部分:

clear,clc
% compute the background image
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 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 : 60
  % load image
  Im = (imread(['DATA/',int2str(i), '.jpg'])); 
  imshow(Im)
  imshow(Im)
  Imwork = double(Im);

  %extract ball
  [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 update

  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


% show positions
  figure
  plot(x(:,2),'r+')
  hold on
  plot(cr,'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;

实验结果:

上图:绿色为观测向量,红色为估计向量。下图为y轴对比,绿色依然为观测值,红色为估计值:

转载注明来源:CV视觉网 » 离散随机线性系统的卡尔曼滤波器基本原理及实现

分享到:更多 ()
扫描二维码,给作者 打赏
pay_weixinpay_weixin

请选择你看完该文章的感受:

0不错 1超赞 0无聊 0扯淡 0不解 0路过

评论 3

评论前必须登录!