没有合适的资源?快使用搜索试试~ 我知道了~
首页ADS1292-呼吸、心率之卡尔曼滤波
ADS1292-呼吸、心率之卡尔曼滤波
3星 · 超过75%的资源 需积分: 30 104 下载量 135 浏览量
更新于2023-03-03
评论 17
收藏 734KB PDF 举报
ADS1292模块的呼吸、心率采集之卡尔曼滤波算法代码,#include "stdlib.h" #include "rinv.c" int lman(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; e=malloc(m*m* sizeof(double )); l=m; if (l<n) l=n; a=malloc(l*l* sizeof (double )); b=malloc(l*l* sizeo
资源详情
资源评论
资源推荐
卡尔曼滤波简介及其算法实现代码
卡尔曼滤波算法实现代码 ( C,C++分别实现)
卡尔曼滤波器简介
近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所
能及的算法。 现在先讨论一下卡尔曼滤波器,如果时间和能力允许,我还希望能够写写其他的算
法,例如遗传算法,傅立叶变换,数字滤波,神经网络,图像处理等等。
所以也只能形象的描述。
因为这里不能写复杂的数学公式,
卡尔曼滤波器 – Kalman Filter
1 、
什么是卡尔曼滤波
器(What is the Kalman Filter?
)
在学习卡尔曼滤波器之前, 首先看看为什么叫“卡尔曼”。 跟其他著名的理论 (例如傅立叶变换,
泰勒级数等等)一样,卡尔曼也是一个人的名字,而跟他们不同的是,他是个现代人!
卡尔曼全名 Rudolf Emil Kalman ,匈牙利数学家, 1930 年出生于匈牙利首都布达佩斯。 1953,
1954 年于麻省理工学院分别获得电机工程学士及硕士学位。 1957 年于哥伦比亚大学获得博士学
位。我们现在要学习的卡尔曼滤波器,正是源于他的博士论文和 1960 年发表的论文《 A New
Approach to Linear Filtering and Prediction Problems 》(线性滤波与预测问题的新方法)。
如果对这编论文有兴趣,可以到这里的地址下载:
http://www.cs.unc.edu/~welch/media/pdf/Kalman1960.pdf 。
简单来说,卡尔曼滤波器是一个“ optimal recursive data processing algorithm (最优化自
回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的
广泛应用已经超过 30 年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统
以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别, 图像分割, 图像边缘检
测等等。
2.卡尔曼滤波器的介绍
(Introduction to the Kalman Filter )
为了可以更加容易的理解卡尔曼滤波器, 这里会应用形象的描述方法来讲解, 而不是像大多数参
考书那样罗列一大堆的数学公式和数学符号。 但是, 他的 5 条公式是其核心内容。 结合现代的计
算机,其实卡尔曼的程序相当的简单,只要你理解了他的那 5 条公式。
在介绍他的 5 条公式之前,先让我们来根据下面的例子一步一步的探索。
假设我们要研究的对象是一个房间的温度。 根据你的经验判断, 这个房间的温度是恒定的, 也就
是下一分钟的温度等于现在这一分钟的温度 (假设我们用一分钟来做时间单位) 。假设你对你的
经验不是 100%的相信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声( White
Gaussian Noise ),也就是这些偏差跟前后时间是没有关系的而且符合高斯分配( Gaussian
Distribution )。另外,我们在房间里放一个温度计,但是这个温度计也不准确的,测量值会比
实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了, 现在对于某一分钟我们有两个有关于该房间的温度值: 你根据经验的预测值 (系统的预测
值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际
温度值。
假如我们要估算 k 时刻的是实际温度值。 首先你要根据 k-1 时刻的温度值, 来预测 k 时刻的温度。
因为你相信温度是恒定的,所以你会得到 k 时刻的温度预测值是跟 k-1 时刻一样的,假设是 23
度,同时该值的高斯噪声的偏差是 5 度( 5 是这样得到的:如果 k-1 时刻估算出的最优温度值的
偏差是 3,你对自己预测的不确定度是 4 度,他们平方相加再开方,就是 5)。然后,你从温度
计那里得到了 k 时刻的温度值,假设是 25 度,同时该值的偏差是 4 度。
由于我们用于估算 k 时刻的实际温度有两个温度值, 分别是 23 度和 25 度。究竟实际温度是多少
呢?相信自己还是相信温度计呢?究竟相信谁多一点,我们可以用他们的 covariance 来判断。
因为 Kg^2=5^2/(5^2+4^2) ,所以 Kg=0.78 ,我们可以估算出 k 时刻的实际温度值是: 23+0.78*
(25-23)=24.56 度。可以看出,因为温度计的 covariance 比较小(比较相信温度计),所以估
算出的最优温度值偏向温度计的值。
现在我们已经得到 k 时刻的最优温度值了, 下一步就是要进入 k+1 时刻, 进行新的最优估算。 到
现在为止,好像还没看到什么自回归的东西出现。 对了, 在进入 k+1 时刻之前, 我们还要算出 k
时刻那个最优值( 24.56 度)的偏差。算法如下: ((1-Kg)*5^2)^0.5=2.35 。这里的 5 就是上面
的 k 时刻你预测的那个 23 度温度值的偏差, 得出的 2.35 就是进入 k+1 时刻以后 k 时刻估算出的
最优温度值的偏差(对应于上面的 3)。
就是这样,卡尔曼滤波器就不断的把 covariance 递归,从而估算出最优的温度值。他运行的很
快,而且它只保留了上一时刻的 covariance 。上面的 Kg,就是卡尔曼增益( Kalman Gain )。他
可以随不同的时刻而改变他自己的值,是不是很神奇!
下面就要言归正传,讨论真正工程系统上的卡尔曼。
3. 卡尔曼滤波器算法
(The Kalman Filter Algorithm )
在这一部分,我们就来描述源于 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 个公
式,可以很容易的实现计算机的程序。
下面,我会用程序举一个实际运行的例子。。。
4. 简单例子
(A Simple Example )
这里我们结合第二第三节, 举一个非常简单的例子来说明卡尔曼滤波器的工作过程。 所举的例子
是进一步描述第二节的例子,而且还会配以程序模拟结果。
根据第二节的描述,把房间看成一个系统, 然后对这个系统建模。 当然, 我们见的模型不需要非
常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以 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)。
最佳线性滤波理论起源于 40 年代美国科学家 Wiener 和前苏联科学家K олмогоров等人的研
究工作, 后人统称为维纳滤波理论。 从理论上说, 维纳滤波的最大缺点是必须用到无限过去的数
据,不适用于实时处理。为了克服这一缺点, 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 时刻动态噪声
T(k,k-1) 为系统控制矩阵
H(k) 为 k 时刻观测矩阵
N(k) 为 k 时刻观测噪声
则卡尔曼滤波的算法流程为:
预估计 X(k)^= F(k,k- 1) ·X(k -1)
1. 计算预估计协方差矩阵
C(k)^=F(k,k- 1) ×C(k) ×F(k,k -1)'+T(k,k- 1) ×Q(k) ×T(k,k -1)'
Q(k) = U(k) ×U(k)'
2. 计算卡尔曼增益矩阵
K(k) = C(k)^ ×H(k)' ×[H(k) ×C(k)^ ×H(k)'+R(k)]^( -1)
R(k) = N(k) ×N(k)'
3. 更新估计
X(k)~=X(k)^+K(k) ×[Y(k) - H(k) ×X(k)^]
4. 计算更新后估计协防差矩阵
C(k)~ =
[I- K(k) ×H(k)] ×C(k)^ ×[I - K(k) ×H(k)]'+K(k) ×R(k) ×K(k)'
5. X(k+1) = X(k)~
C(k+1) = C(k)~
重复以上步骤
剩余20页未读,继续阅读
Chark向
- 粉丝: 6
- 资源: 2
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- zigbee-cluster-library-specification
- JSBSim Reference Manual
- c++校园超市商品信息管理系统课程设计说明书(含源代码) (2).pdf
- 建筑供配电系统相关课件.pptx
- 企业管理规章制度及管理模式.doc
- vb打开摄像头.doc
- 云计算-可信计算中认证协议改进方案.pdf
- [详细完整版]单片机编程4.ppt
- c语言常用算法.pdf
- c++经典程序代码大全.pdf
- 单片机数字时钟资料.doc
- 11项目管理前沿1.0.pptx
- 基于ssm的“魅力”繁峙宣传网站的设计与实现论文.doc
- 智慧交通综合解决方案.pptx
- 建筑防潮设计-PowerPointPresentati.pptx
- SPC统计过程控制程序.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论1