今天介紹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軸對比，綠色依然爲觀測值，紅色爲估計值：
