卡尔曼滤波基础知识doc.docx

卡尔曼滤波基础知识doc.docx

《卡尔曼滤波基础知识doc.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波基础知识doc.docx（17页珍藏版）》请在冰豆网上搜索。

卡尔曼滤波基础知识doc

卡尔曼滤波

马尔可夫过程：

在随机理论中，把在某时•刻的事件受在这之前事件的影响，其影响范围有限的随机过程，称为马尔可夫过程。

一个事件受在它之前的事件的影响的深远程度，通常用在它之前的事件作为条件的概率来表达。

受前一个事件的影响，简称为马尔nJ.夫过程；受前两个事件的影响，称为二阶马尔可夫过程；受前三个事件的影响，称为三阶马尔可夫过程！

卡尔曼滤波简介+算法实现代码（转X

最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家KOJIMOropOB等人的研究工作，后人统称为维纳滤波理论。

从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。

为了克服这一缺点,60年代Kalman把状态空间模型引入滤波理论，并导出了•套递推估计算法，后人称之为卡尔曼滤波理论。

卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求•套递推估计的算法，其基本思想是：

采用信号与噪声的状态空间模型,利用前-时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。

它适合于实时处理利计算机运算。

现设线性时变系统的离散状态防城和观测方程为：

X（k）=F（k,k-1）-X（k-1）+T（k,k-1）U（k-1）

Y（k）=H（k）X（k）+N（k）

其中

X（k）和Y（k）分别是k时刻的状态矢量和观测矢量

F（k,k-1）为状态转移矩阵

U（k）为k!

3寸刻动态噪声

T（k,k・1）为系统控制矩阵

H（k）为k时刻观测矩阵

N（k）为k时刻观测噪声

则卡尔曼滤波的算法流程为：

1

2预估计X（k）A=F（k,k-1）X（k-1）

3计算预估计协方差矩阵C（k）』F（k,k-1）xC（k）xF（k,k-1），+T（k,k-1）xQ（k）xT（k,k-1》Q（k）=U（k）xU（k）,

4计算卡尔曼增益矩阵

K（k）=C（k）AxH（k）,x[H（k）xC（k）AxH（k）,+R（k）]A（-1）

R（k）=N（k）xN（k）,

5更新估计

X（k）-=X（k）A+K（k）x[Y（k）-H（k）xX（k）A]

6计算更新后估计协防差矩阵

C（k）~=[l・K（k）xH（k）]xC（k）Ax[|-K（k）xH（k）T+K（k）xR（k）xK（k》

7X（k+1）=X（k）~

C（k+1）=C（k）~

重夏以上步骤

其C语言实现代码如下：

include“stdlib.h”

include“rinv.c”

intlman（n,m,k,f,q,r,h,y,x,p,g）

intn,m,k;

doublefD,q[],r[],h[]）y[],x[],p[],g[];

（int

double*e,*a,*b;

e=malloc（m*m*sizeof（double））;

l=m;

if（l

a=malloc（rrsizeof（double））;

b=malloc（ri*sizeof（double））;

for（i=0;i<=n-1;i++）

for（j=0;jv=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]*fO*n+kk];

}

for（i=0;iv=n・1;i++）

for（j=0;jv=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++）

（for（i=0;iv=n・1;i++）

for（j=0;j<=m-1;j++）

{jj=i*l+j；a[jj]=O.O；

for（kk=0;kk<=n-1;kk++）

a[jj]=a[jj]+p[i*n+kk]*h『n+kk];

}

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;kkv=n-1;kk++）

e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];

}

js=rinv（e,m）;

if（js==0）

{free（e）;free（a）;free（b）;return（js）;}for（i=0;i<=n-1;i++）

for（j=0;jv=m・1;j++）

{jj=i*m+j;g[jj]=O.O;

for（kk=0;kk<=m-1:

kk++）

g[jj]=g[jj]+a[i*l+kk]*e『m+kk];

for（i=0;iv=n・1;i++）

{jj=（ii-1）F+i;x[jj]=O.O;

for（j=0;jv=n・1;j++）

x[jj]=x[jj]+f[i*n+j]*x[（ii・2）*n+j];

}

for（i=0;i<=m-1;i++）

{jj=i*l；b[jj]=y[（ii・1）*m+i];

for（j=0;jv=n-1;j++）

b[jj]=b[jj]・h[i*n+j]*x[（ii・1）F+j];

}

for（i=0;iv=n・1;i++）

{jj=（ii-1）*n+i;

for（j=0;jv=m.1;j++）

x[jj]=x[jj]+g[i*m+j]*b『l];

）

if（ii

{for（i=0;i<=n-1;i++）

for（j=0;jv=n・1;j++）

{jj=i*l+j；a[jj]=O・O；

for（kk=0;kkv=m-1;kk++）

aDj]=aOj]-g[i*m+kk]*h[kk*n+j];

if（i==j）a[jj]=1.O+a[jj];

}

for（i=0;i<=n-1;i++）

for（j=0;jv=n・1;j++）

{jj=i*l+j；b[jj]=O・O;

for（kk=0;kk<=n-1;kk++）

b[jj]=b[ij]+a[i*l+kk]*p[kk*n+j];

}

for（i=0;i<=n-1;i++）

for（j=O;j<=n-1;j++）

{jj=i*l+j；a[jj]=O.O；

for（kk=0;kk<=n-1;kk++）

a[jj]=a[jj]+b『l+kk]*f『n+kk];

for（i=0;iv=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[iF+kk]*a『l+kk];

}

}

}

free（e）;free（a）;free（b）;

return（js）;

}

C++实现代码如下：

============================kalman.h================================

//kalman.h:

interfaceforthekalmanclass.

//

llllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll

#if!

defined（AFXKALMANHED3D740F01D246168B748BF57636F2C0INCLUDED）

#defineAFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_

#if_MSC_VER>1000

#pragmaonce

#endif//_MSC_VER>1000

include

include"cv.h"

classkalman

（

public:

voidinit_kalman（intxjntxvjntyjntyv）;

CvKalman*cvkalman;

CvMat*state;

CvMat*process_noise;

CvMat*measurement;

constCvMat*prediction;

CvPoint2D32fget_predict（floatx,floaty）;

kalman（intx=0Jntxv=0Jnty=OJntyv=0）;

//virtual~kalman（）;

};

#endif//!

defined（AFXKALMANHED3D740F01D246168B748BF57636F2C0INCLUDED）

============================kalman.cpp================================

#include"kalman.h"

#include

/*testerdeprintertouteslesvaleursdesvecteurs*/

/*testerdechangerlesmatricesdunoises*/

/*replacestatebycvkalman->state_post?

?

?

*/

CvRandStaterng;

constdoubleT=0.1;

kalman:

:

kalman（intxjntxvjnty,intyv）

（

cvkalman=cvCreateKalman（4,4,0）;

state=cvCreateMat（4,1,CV_32FC1）;

process_noise=cvCreateMat（4,1,CV_32FC1）;

measurement=cvCreateMat（4,1,CV_32FC1）;

intcode=-1;

/*creatematrixdata*/

constfloatA[]=（

1,T,0,0,

0,1,0,0,

o,0,1,T,

0,0,0,1

};

constfloatH[]=（

1,0,0,0,

0,0,0,0,

0,0,1,0,

0,0,0,0

}；

constfloatP[]=（

pow（320,2）,pow（320,2）/T,0,0,pow（320,2）/T,pow（320,2）/pow（T,2）,0,0,0,0,pow（240,2）,pow（240,2）/T,

0,0,pow（240,2）/T,pow（240,2）/pow（T,2））；

constfloatQ[]=（

pow（T,3）/3,pow（T,2）/2,0,0,

pow（T,2）/2,T,0,0,

0,0,pow（T,3）/3,pow（T,2）/2,

0,0,pow（T,2）/2,T

）；

constfloatR[]=（

1,0,0,0,

0,0,0,0,

0,0,1,0,

0,0,0,0

）;

cvRandlnit（&rng,0,1,-1,CV_RAND_UNI）;

cvZero（measurement）;

cvRandSetRange（&rng3030.1,0）;

rng.disttype=CV_RAND_NORMAL;

cvRandf&rng,state）;

memcpy（cvkalman->transition_matrix->data.fl,A,sizeof（A））;

memcpy（cvkalman->measurement_matrix->data.fl,H,sizeof（H））;

memcpy（cvkalman->process_noise_cov->data.fl,Q,sizeof（Q））;

memcpy（cvkalman->error_cov_post->data.fl:

P,sizeof（P））;

memcpy（cvkalman->measurement_noise_cov->data.fl,R,sizeof（R））;

//cvSetldentity（cvkalman->process_noise_cov,cvRealScalar（1e-5））;

//cvSetldentity（cvkalman->error_cov_post,cvRealScalar（l））;

//cvSetldentity（cvkalman->measurement_noise_cov5cvRealScalar（1e-1））;

/*chooseinitialstate*/

state->data.fl[O]=x;

state->data.fl[1]=xv;

state->data.fl[2]=y;

state->data.fl[3]=yv;

cvkalman->state_post->data.tl[0]=x;

cvkalman->state_post->data.fl[1]=xv;

cvkalman->state_post->data.fl[2]=y;

cvkalman->state_post->data.fl[3]=yv;

cvRandSetRange（&rng,0,sqrt（cvkalman->process_noise_cov->data.fl[0]）,0）;

cvRand（&rng,process_noise）;

CvPoint2D32fkalman:

:

get_predict（floatx,floaty）（

/*updatestatewithcurrentposition7state->data.fl[O]=x;

state->data.fl[2]=y;

/*predictpointposition*/rx'k=AStk+Bfekk

P,k=Ak-1*AT+Q7

cvRandSetRange（&rng,0,sqrt（cvkalman->measurement_noise_cov->data.fl[0]）,0）;

cvRandf&rng,measurement）;

/*xk=A?

xk-1+B?

uk+wk*/

cvMatMulAdd（cvkalman->transition_matrix3state,process_noise,cvkalman->state_post）;

/*zk=H?

xk+vk*/

cvMatMulAdd（cvkalman->measurement„matrix,cvkalman->state_post,measurement,measurement）;

cvKalmanCorrect（cvkalman,measurement）;

floatmeasured_value_x=measurement->data.fl[O];

floatmeasured_value_y=measurement->data.fl[2];

constCvMat*prediction=cvKalmanPredict（cvkalman,0）;

floatpredict_value_x=prediction->data.tl[0];

floatpredict_value_y=prediction->data.fl[2];

return（cvPoint2D32f（predict_value_x?

predict_value_y））;

）

voidkalman:

:

init_kalman（intxjntxvjntyjntyv）

（

state->data.fl[O]=x;

state->data.fl[1]=xv;

state->data.fl[2]=y;

state->data.fl[3]=yv;

cvkalman->state_post->data.tl[0]=x;

cvkalman->state_post->data.fl[1]=xv;

cvkalman->state_post->data.fl[2]=y;

cvkalman->state_post->data.fl[3]=yv;

）

白噪声是的特点是功率谱为常数，也就是各个频率成分幅值相同，无法用传统的低通、高通、带通、带阻滤波器去除。

如果考察信号为周期信号，通过多周期平均来消除白噪声是比较好的办法，如果被考察信号为非周期信号，则需要别的方法，如小波去噪，EMD去噪，自适应滤波等。

小波阈值设置好的话，效果可以，EMD我试过，SNR低了效果不行，自适应我不太了解。

均值就是所有数的平均数，就是把所有数都加起来再除以个数

方差就是把每个数减去它们的平均数再平方，把这些平方加起来再除以个数方差表示统计数据的离散程度

卡尔曼滤波器-KalmanFilter

1.什么是卡尔曼滤波器

（WhatistheKalmanFilter?

）

在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！

卡尔曼全名RudolfEmilKalman,匈牙利数学家,1930年出生于匈牙利首都布达佩斯。

1953,1954年于麻省理工学院分别获得电机工程学士及硕士学位O1957年于哥伦比亚大学获得博士学位。

我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《ANewApproachtoLinearFilteringandPredictionProblems^线性滤波与预测问题的新方法）。

如果对•这编论文有兴趣,可以到这里的地址下载：

简单来说,卡尔曼滤波器是…个''optimalrecursivedataprocessingalgorithm（最优化自回归数据处理算法）飞对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。

他的广泛应用己经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。

近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2.卡尔曼滤波器的介绍

（IntroductiontotheKalmanFilter）

为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。

但是，他的5条公式是其核心内容。

结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

在介绍他的5条公式之前，先让我们来根据下面的例子一•步一•步的探索。

假设我们要研究的对象是一个房间的温度。

根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用…分钟来做时间单位）。

假设你对你的经验不是100%的相信，可能会有上下偏差几度。

我们把这些偏差看成是高斯白噪声（WhiteGaussianNoise）,也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（GaussianDistribution）□另夕卜，我们在房间里放一个温度计,但是这个温度计也不准确的，测量值会比实际值偏差。

我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：

你根据经验的预测值（系统的预测值）和温度计的值（测量值）。

下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。

首先你要根据k-1时刻的温度值，来预测k时刻的温度。

因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的:

如果k-1时刻估算出的最优温度值的偏差是3,你对IH己预测的不确定度是4度，他们平方相加再开方，就是5）。

然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。

究竟实际温度是多少呢？

相信自己还是相信温度计呢？

究竟相信谁多一点，我们可以用他们的covariance来判断。

因为KgA2=5A2/（5A2+4A2）,所以Kg=0.78,我们可以估算出k时刻的实际温度值是:

23+0.78*（25-23）=24.56度。

可以看出,因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们己经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。

到现在为止，好像还没看到什么自回归的东西出现。

对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。

算法如下:

（（1-Kg）*5八2）的.5=2.35。

这里的5就是上面的k时刻你预测的那个23度湿度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。

他运行的很快，而且它只保留了上一时刻的covarianceo上面的Kg,就是卡尔曼增益（KalmanGain）。

他可以随不同的时刻而改变他己的值，是不是很神奇！

下面就要言归正传，讨论真正工程系统上的卡尔曼。

3.卡尔曼滤波器算法

（TheKalmanFilterAlgorithm）

在这一部分，我们就来描述源于DrKalman的卡尔曼滤波器。

下面的描述，会涉及一些基本的概念知识，包括概率（Probability）,随即变量（RandomVariable）,高斯或正态分配（GaussianDistribution）还有State-spaceModel等等。

但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。

该系统可用一•个线性随机微分方程（LinearStochasticDifferenceequation）来描述：

X（k）=AX（k・1）+BU（k）+W（k）

再加上系统的测量值：

Z（k）=HX（k）+V（k）

上两式子中，X（k）是k时刻的系统状态，U（k）是k时刻对系统的控制量。

A和B是系统参数，对于多模型系统，他们为矩阵。

Z（k）是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。

W（k）和V（k）分别表示过程和测量的噪声。

他们被假设成高斯白噪声（WhiteGaussianNoise）,他们的covariance分别是Q,R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件（线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。

下面我们来用他们结合他们的covariances来