Kanlman滤波器学习(原理及简单例程)
来源:互联网 发布:linux 限制进程cpu 编辑:程序博客网 时间:2024/05/23 23:37
卡尔曼全名Rudolf Emil Kalman,匈牙利数学家,1930年出生于匈牙利首都布达佩斯。1953,1954年于麻省理工学院分别获得电机工程学士及硕士学位。
1957年于哥伦比亚大学获得博士学位。我们在现代控制理论中要学习的卡尔曼滤波器,正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预测问题的新方法)。
1.卡尔曼滤波器算法
在这一部分,我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述,会涉及一些基本的概念知识,包括概率(Probability),随即变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model等等。但对于卡尔曼滤波器的详细证明,这里不能一一描述。
首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述:
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值:
Z(k)=H X(k)+V(k)
上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance 分别是Q,R(这里我们假设他们不随系统状态变化而变化)。
对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。
首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。
到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance还没更新。我们用P表示covariance:
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的covariance,A’表示A的转置矩阵,Q是系统过程的covariance。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。
现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k):
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain):
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance:
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
其中I 为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1)。这样,算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了,式子1,2,3,4和5就是他的5 个基本公式。根据这5个公式,可以很容易的实现计算机的程序。
下面,我会用程序举一个实际运行的例子
2.简单例子
这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。
根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以A=1。没有控制量,所以U(k)=0。因此得出:
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子(2)可以改成:
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因为测量的值是温度计的,跟温度直接对应,所以H=1。式子3,4,5可以改成以下:
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)
现在我们模拟一组测量值作为输入。假设房间的真实温度为25度,我模拟了200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声(在图中为蓝线)。
为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。我选了X(0|0)=1度,P(0|0)=10。
该系统的真实温度为25度,图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果(该结果在算法中设置了Q=1e-6,R=1e-1)。
N=200;
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;
p1(t)=a.^2*p(t-1)+Rww; % P(k|k-1) = A*P(k-1|k-1)A' + Q
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); %卡尔曼增益矩阵Kg = P(k|k-1)H'/(H*P(k|k-1)+R)
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); %最优状态X(k|k) = A*X(k|k-1) + Kg(k)*(Z(k)-H*X(k|k-1))
p(t)=p1(t)-c*b(t)*p1(t); %协方差的更新P(k|k) = (I-Kg(k)*H)*P(k|k-1)
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');
用matlab做的kalman滤波程序,以通过测试
卡尔曼滤波源程序
#include "stdlib.h"
int klman(n,m,k,f,q,r,h,y,x,p,g)
int n,m,k;
double f[],q[],r[],h[],y[],x[],p[],g[];
{ int i,j,kk,ii,l,jj,js;
double *e,*a,*b;
extern int brinv();
e=malloc(m*m*sizeof(double));
l=m;
if (l<n) l=n;
a=malloc(l*l*sizeof(double));
b=malloc(l*l*sizeof(double));
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];
}
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++)
{ 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];
}
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);}
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];
}
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];
}
for (i=0; i<=m-1; i++)
{ jj=i*l; b[jj]=y[(ii-1)*m+i];
for (j=0; j<=n-1; j++)
b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];
}
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)
{ 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];
if (i==j) a[jj]=1.0+a[jj];
}
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];
}
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];
}
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);
}
转自:http://jpkc.nwpu.edu.cn/jp2005/40/ebook/kcsj/chp1/Kalman.htm
有用链接:http://www.cs.unc.edu/~welch/kalman/
- Kanlman滤波器学习(原理及简单例程)
- 机器学习算法原理与实践(三)、卡尔曼滤波器算法浅析及matlab实战
- 双边滤波器原理及实现
- Opencv学习笔记(一)滤波器原理及其使用
- 有源滤波器和无源滤波器的区别及原理
- 例程创建及原理 (摘自网络博客)
- 双边滤波器的原理及实现
- 双边滤波器的原理及实现
- 双边滤波器的原理及实现
- 双边滤波器的原理及实现
- 双边滤波器的原理及实现
- 双边滤波器的原理及实现
- 卡尔曼滤波器的原理及应用
- 双边滤波器的原理及实现
- 双边滤波器原理及Matlab实现
- 双边滤波器的原理及实现
- java例程练习(Point类及简单测试)
- openFrameworks 学习笔记(一): 简单例程分析
- 到底调用什么方法呢
- 数学之美 系列二 谈谈中文分词
- C#中的MD5加密函数
- 用Java实现自动在数据库表中生成ID号
- 数学之美 系列三 隐含马尔可夫模型在语言处理中的应用
- Kanlman滤波器学习(原理及简单例程)
- ASP.NET 2.0 HttpHandler实现生成图片验证码(示例代码下载)
- 数学之美 系列四 怎样度量信息
- 数学之美 系列五 简单之美:布尔代数和搜索引擎的索引
- 数学之美 系列六 图论和网络爬虫 (Web Crawlers)
- 数学之美 系列七 信息论在信息处理中的应用
- Java中实现基于Aspectwerkz的AOP开发
- 数学之美 系列八 贾里尼克的故事和现代语言处理
- 数学之美 系列九 如何确定网页和查询的相关性