离散随机线性系统的卡尔曼滤波器基本原理及实现
今天介绍Kalman滤波器理论知识，并给出一个演示的例子。由于Kalman滤波在目标跟踪时，需要不断获取观测向量，所以没法单独使用。如果时间充裕，下一篇博文将会做基于MeanShift + Kalman的目标跟踪。这次的主要结构：
1. 卡尔曼滤波器基本原理
2. 卡尔曼滤波器算法
3. 演示例子【来自课本：C语言常用算法程序集(第二版)】+网上广为流传的自由落体小球跟踪matlab
一、离散随机线性系统的卡尔曼滤波器基本原理
卡尔曼滤波方法是一种递推的状态空间方法，其基本特征是利用状态方程描述状态的转移过程，使用观测方程获得对状态的观测信息。它只用状态的前一个估计值和最近一个观察值就可以在线性无偏最小方差估计准则下对当前状态作出最优估计。【对于时变系统来说，状态是关于时间的变量】
【这里，我特别强调一下观察值，必须更新，否则后验估计将不准确（比如突然改变运动方向）。Opencv的跟踪鼠标例子、和网上流传的小球自由落体跟踪，它的观测值始终在更新】
下面，首先给出描述离散时变线性系统的状态方程和观测方程：
其中，xk为k时刻系统特征的状态向量，zk为观测向量，A为状态由k-1时刻到k时刻的转移矩阵，uk-1为k-1时刻可选输入控制矩阵，B为可选输入控制矩阵的增益矩阵【这边可选的就不选了】，H表示状态向量xk对观测向量zk的增益，通常为常数矩阵。wk和vk分别表示k时刻系统输入随机噪声向量和观测噪声向量，通常为常数矩阵。
卡尔曼滤波器要解决的基本问题就是如何在k时刻已知观测变量zk的情况下求解状态向量xk线性最小方差估计。卡尔曼滤波器将状态视为抽象状态空间中的点，进而利用在Hilbert空间中定义的投影理论解决状态的最优估计问题。【数学推导略】
二、离散卡尔曼滤波器算法
卡尔曼滤波器分为两个部分：状态预测方程（状态预估）和状态修正方程（观测更新）。状态预测方程负责及时向前推算当前状态变量和误差协方差的估计值，以便为下一时间状态构造先验估计。状态修正方程负责反馈，即将先验估计和新的观测变量结合以构造改进的后验估计。
下面对时间离散点上的采样数据进行卡尔曼滤波的公式进行完整梳理。
设n为线性动态系统和m维线性观测系统由下列方程描述：
。 xk为n为向量，表示系统在第k时刻的状态
。 A是一个n*n阶矩阵，成为系统的状态转移矩阵，它反映了系统从第k-1个采样时刻的状态到第k个采样时刻的状态变换
。 wk-1是一个n为向量，表示在第k时刻作用于系统的随机干扰，称为模型噪声，为简单起见，一般假设wk为高斯白噪声序列，具有已知的零均值和协方差阵Qk
。 zk为m维观测向量
。 H为m*n阶观测矩阵，表示了从状态xk到观测量zk的转换
。 vk为m维观测噪声，同样假设vk为高斯白噪声序列，具有已知的零均值和协方差阵Rk
经过推导【略】，可得如下滤波的递推公式：
。Qk为n*n阶的模型噪声wk的协方差阵
。Rk为m*m阶的观测噪声vk的协方差阵
。Kk为n*m阶增益矩阵
。为n维向量，第k时刻经过滤波后的估值
。Pk为n*n阶的估计误差协方差阵
根据上述公式，可以从x0=E{x0}，给定的P0出发，利用已知的矩阵Qk，Rk，H，A以及k时刻的观测值zk，递推地算出每个时刻状态的估值
如果线性系统是定常的，则A和H都是常数矩阵；如果模型噪声wk和观测噪声vk都是平稳随机序列，则Qk和Rk为常数矩阵。在这种情况下，常增益的离散卡尔曼滤波是渐近稳定的。
下面给出卡尔曼滤波器工作原理图：
三、演示例子
实验一：
来自《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<n) l=n;
a= (double *) malloc(l*l*sizeof(double));
b= (double *)malloc(l*l*sizeof(double));
//程序块a和b：计算了P=APA'
//a
for (i=0; i<=n-1; i++){
for (j=0; j<=n-1; j++)
{
ii=i*l+j; a[ii]=0.0;
for (kk=0; kk<=n-1; kk++)
a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];
}
}
//b
for (i=0; i<=n-1; i++){
for (j=0; j<=n-1; j++)
{
ii=i*n+j; p[ii]=q[ii];
for (kk=0; kk<=n-1; kk++)
p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];
}
}
for (ii=2; ii<=k; ii++)
{
//程序块c和d：计算了HPH'+R
//c
for (i=0; i<=n-1; i++){
for (j=0; j<=m-1; j++)
{
jj=i*l+j; a[jj]=0.0;
for (kk=0; kk<=n-1; kk++)
a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];
}
}
//d
for (i=0; i<=m-1; i++){
for (j=0; j<=m-1; j++)
{
jj=i*m+j;
e[jj]=r[jj];
for (kk=0; kk<=n-1; kk++)
e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];
}
}
js=brinv(e,m);
if (js==0)
{
free(e); free(a); free(b); return(js);
}
//程序块c,d,e:K=PH'(HPH'+R)^-1
//e
for (i=0; i<=n-1; i++){
for (j=0; j<=m-1; j++)
{
jj=i*m+j; g[jj]=0.0;
for (kk=0; kk<=m-1; kk++)
g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];
}
}
//程序块f,g,h：x= x+K(z-Hx)
//f
for (i=0; i<=n-1; i++)
{
jj=(ii-1)*n+i; x[jj]=0.0;
for (j=0; j<=n-1; j++)
x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j]; //x=Ax+w,w=0
}
//g
for (i=0; i<=m-1; i++)
{
jj=i*l;
b[jj]=y[(ii-1)*m+i]; //z=y,观测向量为信号源运动方程
for (j=0; j<=n-1; j++)
b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];
}
//h
for (i=0; i<=n-1; i++)
{
jj=(ii-1)*n+i;
for (j=0; j<=m-1; j++)
x[jj]=x[jj]+g[i*m+j]*b[j*l];
}
if (ii<k) //只算200个采样点
{
//程序块i,j：P=(I-KH)P
//i
for (i=0; i<=n-1; i++){
for (j=0; j<=n-1; j++)
{
jj=i*l+j;
a[jj]=0.0;
for (kk=0; kk<=m-1; kk++)
a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j]; //KH
if (i==j)
a[jj]=1.0+a[jj]; //I-KH
}
}
//j
for (i=0; i<=n-1; i++){
for (j=0; j<=n-1; j++)
{
jj=i*l+j;
b[jj]=0.0;
for (kk=0; kk<=n-1; kk++)
b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j]; //(I-KH)P
}
}
//程序块k,l：P= APA'+ Q
//k
for (i=0; i<=n-1; i++){
for (j=0; j<=n-1; j++)
{
jj=i*l+j;
a[jj]=0.0;
for (kk=0; kk<=n-1; kk++)
a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk];
}
}
//l
for (i=0; i<=n-1; i++){
for (j=0; j<=n-1; j++)
{
jj=i*n+j;
p[jj]=q[jj];
for (kk=0; kk<=n-1; kk++)
p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk];
}
}
}
}
free(e); free(a); free(b);
return(js);
}
矩阵求逆函数：
#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轴对比，绿色依然为观测值，红色为估计值：
