卡尔曼滤波算法与matlab实现

一个应用实例详解卡尔曼滤波及其算法实现 标签 算法 filtermatlabalgorithm 优化工作 2012 05 14 10 48 75511 人阅读 评论 25 收藏 举报 分类 数据结构及其算法 4 为了可以更加容易的理解卡尔曼滤波器 这里会应用形象的描述方法来讲解 而不是像大多数参考书那样罗列一大堆的数学公式和数学符号 但是 他的 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 个公式 可以很容易的实现计算机的程序 下面 用 Matlab 程序举一个实际运行的例子 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 clear 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 b t c p1 t c 2 p1 t Rvv s t a s t 1 b t Y t a c s t 1 p t p1 t c b t p1 t end t 1 N plot t s r t Y g t x b 用 matlab 做的 kalman 滤波程序 已通过测试 还有下面一个 Matlab 源程序 显示效果更好 clear clc N 300 CON 25 房间温度 假定温度是恒定的 kalman filter x zeros 1 N y 2 0 5 randn 1 N CON 加过程噪声的状态输出 x 1 1 p 10 Q cov randn 1 N 过程噪声协方差 R cov randn 1 N 观测噪声协方差 for k 2 N x k x k 1 预估计 k 时刻状态变量的值 p p Q 对应于预估值的协方差 kg p p R kalman gain x k x k kg y k x k p 1 kg p end Smoothness Filter Filter Wid 10 smooth res zeros 1 N for i Filter Wid 1 N tempsum 0 for j i Filter Wid i 1 tempsum tempsum y j end smooth res i tempsum Filter Wid end figure 1 hist y t 1 N figure 1 expValue zeros 1 N for i 1 N expValue i CON end plot t expValue r t x g t y b t smooth res k legend expected estimate measure smooth result axis 0 N 20 30 xlabel Sample time ylabel Room Temperature title Smooth filter VS kalman filter 卡尔曼滤波算法卡尔曼滤波算法 核心公式推导导论核心公式推导导论 再造红旗 写在最前面 这是我第一篇专栏文章 感谢知乎提供这么一个平台 让自己能 和大家分享知识 本人会不定期的开始更新文章 文章的内容应该集中在汽车 动力学控制 整车软件架构 控制器等方面 作为一名在校硕士 很多理解都 可能不全面 不正确 大家有不同意见欢迎讨论 谢谢 卡尔曼滤波算法很牛逼 因为有一堆公式 有一堆符号 看起来就很牛逼啊 乍一看不懂的都很牛逼啊 本文针对卡尔曼滤波算法的核心公式进行推导 不让大家被它华丽的外表吓到 之后计划写关于针对非线性情况的 EKF 和 UKF 对卡尔曼滤波算法做一个 全面一点的应用介绍 感兴趣的可以关注专栏 Okay 进入正题 这篇文章假设读者已经对卡尔曼滤波算法有初步的了解 知 道它能做什么 知道它的优点 知道它很牛逼 并且你已经对它产生兴趣 但 不知道如何下手 首先给出一个控制理论中公式 别急着翻控制理论的书 没那么复杂 两个基本问题 1 卡尔曼滤波算法要做什么 对状态进行估计 2 卡尔曼滤波算法怎么对状态进行估计 利用状态过程噪声和测量噪声对状态进行估计 一个状态在一个时刻点 k 的状态进入下一个时刻点 k 1 状态 会有很多外界因 素的干扰 我们把干扰就叫做过程噪声 这个词一看就是硬翻译过来的 别 在意为什么叫噪声 用 w 表示 任何一个测量仪器 都会有误差 我们把这个 误差叫做量测噪声 用 v 表示 回到上面那个公式 状态方程表示状态在不断的更新 从一个时刻点进入下一 个时刻点 这个很好理解 关键是量测方程 它表示 我们不断更新的状态有 几个能用测量仪器测出来 比如 汽车运动状态参数有很多 比如速度 轮速 滑移率等 但是我们只能测量出轮速 因此量测方程要做的就是把状态参数中 能量测的状态拿出来 我们始终要记得我们要做的事 我们要得到的是优化的状态量 Xk 理解了上面之后就可以开始推导公式了 1 首先不考虑过程噪声对状态进行更新 很简单 举个例子 v k v k 1 at 匀加速运动咯 2 不考虑测量噪声取出能测量的状态 也很简单 3 用测量仪器测量出来的状态值 大家可以考虑 到 测量的值就是被各种噪声干扰后的真实值 减去上面不考虑噪声得到的测 量值 这个值在数学上是一个定义值 叫做新息 有很多有趣的性 质 感兴趣的可以自己谷歌 我们对步骤暂且停一停 这个叫新息的值有什么用 由上面的过程我们可以明 显看到 它反映了过程噪声和测量噪声综合对测量状态值的影响 也就是它包 含了 w 和 v 的情况 回到数学层面 不要害怕 很简单的数学应用和思考啦 一个数值 c 由两 部分内容 a 和 b 组成 那么怎样用数学表达式来表达 一般有两种做法 I 直接相加 c a b II 用比例的方法 a n c b 1 n c 卡尔曼采用了方法 II 用比例的方法来做 其实这也是为什么叫做滤波的原因 因为滤波就是给权值之类的操作 也就是说 过程噪声 w 新息 一个比例 这样得到的过程噪声加上原来 第一步 不考虑过程噪声的状态值不就是优化 值了吗 也就是 Okay 都写到这里了 有必要做一下前提假设 a 什么高斯噪声 均值为零一堆 b Ak Ck wk 的协方差 Q vk 的协方差 R 系统协方差初始值 P0 状态初始值 X0 都已知 为什么已知 你实际做项目就知道了 不过不懂的可以留言或者 私信 那么到目前为止我们的思路就是清楚了 找到一个合适的 Hk 值 卡尔曼增益 那么我们就能得到状态的最优值 卡尔曼说的 不是我说的 所以你问为 什么 你要问他 这么深层次的理论留给博士和学者们去做就好 我们就现学 现用就行 哈哈哈 站在巨人的肩膀 问题来了 怎么得到合适的 Hk 似乎不是随便一个参数 这是误差协方差矩阵 思路 使得误差协方差矩阵 Pk 最小的 Hk 为什么 这里我从感观的角度说明自己的理解 欢迎讨论 协方差表示什么 协方差表示两者之间的联系或者关系 关系越大 协方差越 大 误差协方差越小说明过程噪声和量测噪声的关系越小 关系越小能做什么 这要回到我们第 3 步讨论的我们用比例的方法分开了 w 和 v 用比例分开 到 底多少属于 w 多少是 v 如果关系越小 分开的越精确 比如一堆白砂糖和 盐 如果两种混合的很均匀 我们说它关系很大 也就越难用比例的方法将其 分开 4 求的误差协方差矩阵 Pk 自然是把里面的 Xk 先得到 然后公式运算 通过上面的 步骤我们也容易得到 然后复杂的数学计算 和之前假设的高斯噪声 新息的性质之类 至于过程 个人觉得你如果只做应用 不研究算法 就没必要深入去看了 就能得到下 面的卡尔曼滤波递推公式 通过上面的解释 我们也就不难知道这些公式都在干嘛 知道干嘛就可以了 在知道 A C P0 Q R 的情况下 整个公式的运算流程也都很清晰了 过程方程 X k 1 A X k B U k W k 式 1 量测方程 Z k 1 H X k 1 V k 1 式 2 A 和和 B 是系统参数 对于多模型系统 他们为矩阵 是系统参数 对于多模型系统 他们为矩阵 H 是测量系统的参数 对于多测量系是测量系统的参数 对于多测量系 统 统 H 为矩阵 为矩阵 W k 和和 V k 分别表示过程和测量的噪声 他们被假设成高斯白噪声 他们分别表示过程和测量的噪声 他们被假设成高斯白噪声 他们 的协方差的协方差 分别是分别是 Q R 为了不失一般性 下面的讨论中将 为了不失一般性 下面的讨论中将 X Z 都视为矩阵 其中都视为矩阵 其中 X 是是 m 行的单列矩阵 行的单列矩阵 Z 是是 n 行的单列矩阵 行的单列矩阵 说明 下面的表达式中 不带前缀的量都代表实际量 其小括号里面的 k 或 k 1 代表该 量是第 k 或第 k 1 时刻的实际量 如 Z k 1 就代表第 k 1 时刻的实际测量值 带前缀 的量都代表预测量 如果小括号里面是 k 1 k 就代表 k 1 时刻的先验预测值 如果小括号里面是 k 1 k 1 就代表 k 1 时刻的后验预测值 测量值可以通过测量得到 所以只有先验预测 没有后验预测 而实际状态值无法得知 既有先验预测 又有后验预 测 带前缀 的量都代表与预测值对应的偏差值 实际状态值与先验预测状态值的偏差 实际状态值 先验预测状态值 X k 1 k X k 1 X k 1 k 式 3 实际测量值与先验预测测量值的偏差 当前测量值 先验预测测量值 Z k 1 k Z k 1 Z k 1 k 式 4 并且 先验预测测量值 转换矩阵 H 先验预测状态值 Z k 1 k H X k 1 k 式 5 得到测量值后 再对当前状态值 X k 1 进行后验预测 设后验预测值为 Z k 1 k 1 则后验预测值 同时也是最终预测值 的偏差为 X k 1 k 1 X k 1 X k 1 k 1 式 6 为了得到当前状态值 X k 1 根据式 3 需要 X k 1 X k 1 k X k 1 k 式 7 上式中 我们可以通过卡尔曼公式 1 见附注 2 计算出 X k 1 k 但我们无法得知实际实际 状态值状态值 X k 1 因而 X k 1 k 也无法得知 我们最终的目的是得出一个比较接近实际状实际状 态值态值 X k 1 的滤波值 X k 1 k 1 根据式 7 只要能准确的估计出 X k 1 k 即可 X k 1 k 本身虽无法得知 但 Z k 1 k 却可以通过测量得到 而且它们二者存在一定的 相关性 不妨再设存在一个矩阵 K m 行 n 列矩阵 能使得 X k 1 k K Z k 1 k 式 8 那么最终的预测任务其实就是找到那么最终的预测任务其实就是找到 K 由于 X k 1 k 和 Z k 1 k 都是单列矩阵 因此不难 看出 满足式 8 的矩阵 K 应有无穷多个 矩阵 K 中第 i 行第 j 列反映了量测变量偏差矩阵 Z k 1 k 的第 j 个元素对状态变量偏差矩阵 X k 1 k 的第 i 个元素的贡献 因此矩阵 K 的 物理意义很明显 K 的第 i 行第 j 列的元素表示 对于第 i 个待测的状态量来说 第 j 个测 量仪器测到的偏差的可信度 某个测量值对应的可信度越高 滤波器越 相信 该测量值 既然满足条件的 K 有无穷多个 那应该使用哪个 K 呢 实际上 我们并不知道 X k 1 k 的值 所以也就无法直接计算出 K 而只能通过某种方法找到一个 Kg 使得将 Kg 带入式 8 后 等号两边的差 的平方 的期望尽可能小 我们最终的预测值或滤波值是后验预测值 X k 1 k 1 因此最后的预测也应使 X k 1 k 1 的期望为 0 且方差最小 这与让 8 式两端的差最小是一致的 下面的式 9 体 现了这一点 这样预测值才最可靠 下面详细说明 X k 1 k 1 X k 1 k Kg Z k 1 k 后验预测的状态值 X k 1 k 1 X k 1 X k 1 k 1 后验预测的偏差 X k 1 k 1 X k 1 X k 1 k 1 X k 1 k X k 1 k X k 1 k Kg Z k 1 k X k 1 k Kg Z k 1 k 式 9 Z k 1 k Z k 1 Z k 1 k H X k 1 V k 1 H X k 1 k H X k 1 X k 1 k V k 1 H X k 1 k V k 1 式 10 接下来的分析中 为了更直观的说明卡尔曼滤波的原理 我们用几何方法来解释 这时 X 和 Z 矩阵中的每个元素应看做向量空间中的一个向量而不再是一个单纯的数 这个向 量空间 统计测试测试空间 可以看成无穷多维的 每一个维对应一个可能的状态 X 和 Z 矩阵中的每个元素向量都是由所有可能的状态按照各自出现的概率组合而成 在测量之前 X 和 Z 的实际值都是不可知的 X 和 Z 中的每个元素向量都应是 0 均值的 他们与 自己的内积就是他们的协方差矩阵 我们无法给出 X 和 Z 中每个元素向量的具体表达 但我们通过协方差矩阵就可以知道所有元素向量的模长 以及相互之间的夹角 从内积计 算 为了方便用几何方法解释 我们假设状态变量 X 是一个 1 行 1 列的矩阵 即只有一个待测 状态量 而量测变量 Z 是一个 2 行 1 列的矩阵 即有两个测量仪器 共同测量同一个状 态量 X 也就是说 m 1 n 2 矩阵 X 中只有 X 1 一项 矩阵 Z 中有 Z 1 和 Z 2 两项 Kg 此时应是一个 1 行 2 列的矩阵 两个元素分别记作 Kg1 和 Kg2 H 和 V 此时应是一 个 2 行 1 列的矩阵 将矩阵表达式 9 和 10 按元素展开 X k 1 k 1 1 X k 1 k 1 Kg1 Z k 1 k 1 Kg2 Z k 1 k 2 式 9i Z k 1 k i H i X k 1 k V k 1 i 式 10i X k 1 k 中各个元素 向量 的线性组合可以产生一个 m 维或更低维的向量子空间 Vx 这里 按照我们的假设 m 1 所以 Vx 应是一维的 同时 V k 1 中的各个元素 向量 的线性组合也可以产生一个 n 维或更低维的向量子空间 Vv 这里 按照我们的假 设 n 2 所以 Vv 应是二维的 由于 V k 1 中的每一项与 X k 1 k 中的每一项都不相关 见附注 1 故这两个子空间相互垂直 如下图所示 式 10i 所体现的 Z k 1 k i H i X k 1 k V k 1 i 三者之间的几何关系 也在下图中描绘了出来 从上图中可以看出 Z k 1 k 中各个元素 向量 的线性组合也可以产生一个 n 维或更 低维的向量子空间 Vz 这里已假设 n 2 所以 Vz 是一个二维的平面 就是上图中两条红 色的线所构成的平面 图 2 中 注意此图中的椭圆代表的是 Vz 空间 而图 1 中则代表 Vv 空间 二 者不一样 粉色的向量就是 Kg1 Z k 1 k 1 Kg2 Z k 1 k 2 记此 粉色向量为 Y Y 为 Z k 1 k 1 和 Z k 1 k 2 线性组合而成 它始终在子空 间 Vz 中 根据式 9i X k 1 k 1 1 等于 X k 1 k 1 和 Y 的差向量 为使 X k 1 k 1 1 长度最短 协方差最小 Kg 的选取应使得 X k 1 k 1 1 垂 直于 Vz 空间空间 通过先验预测的协方差矩阵 见卡尔曼公式 2 可以得到 X k 1 k 中各个元 素的模长以及彼此间的夹角 这是因为协方差矩阵中的第 i 行第 j 列其实就代表 了 X k 1 k 中第 i 个元素向量与第 j 个元素向量的内积 通过测量可以得到新息协方差 见卡尔曼公式 3 的分母部分 进而可以知道 Z k 1 k 中各个元素的模长以及彼此间的夹角 通过已知的量测噪声协方差矩阵 R 可以得出 V k 1 中各个元素的模长以及 彼此间的夹角 最后根据 X k 1 k 1 1 与 Y 垂直以及图 1 中所示的几何关系 用高中学的立 体几何和向量知识就可以求得两个 Kg 的值了 如果将向量的内积都用协方差 矩阵表示 就会发现 我们最后求得的 Kg 其实就是卡尔曼公式 3 上面讨论的是较低次的卡尔曼滤波 只有一个待测量 两个测量仪器 这种 情况还是比较常见的 比如倾角测量系统中 我们用加速度计和陀螺仪共同测 量倾角 对于更高次的卡尔曼滤波 X 和 Z 都是多行矩阵时 用几何方法已经 无法直观解释 只能用矩阵分析的方法证明 求解 Kg 的详细过程参考 卡尔 曼滤波器及其应用基础 国防工业出版社敬喜 编 卡尔曼滤波的核心过程 就是求解能使得 E X k 1 k 1 X k 1 k 1 取最小值的 Kg 增益矩阵的过程 X k 1 k 1 代表的是 X k 1 k 1 的转置 这里 X k 1 k 1 中的元素代表数值 不是向量 前面已经提到过 卡尔曼 增益矩阵 Kg 中的元素 都代表测量仪器测到的偏差的可信度 或者叫估计权 重 附注 1 a v k 1 中的每一项与 X k 1 k 中的每一项都不相关 X k 1 k X k 1 X k 1 k X k 1 A X k k B U k A X k B U k W k A X k B U k A X k k W k A X k k W k A X k k 1 Kg k Z k k 1 这一步利用了 式 9 W k A X k k 1 Kg k H X k k 1 v k 这一步利用了式 10 W k A Kg k v k A I Kg k H X k k 1 上式最后一行出现了 X k k 1 可见 X k 1 k 可以递归表示 而且递归式中的 过程噪声 W k 与 v k 1 不相关 同时由于 v 本身是白噪声 所以 v k 1 与 v k 亦 不相关 白噪声的自相关是 函数 因此通过递推式可以判断 v k 1 与 X k 1 k 不相关 b w k 1 中的每一项与 X k 1 k 1 中的每一项都不相关 w k 1 中的每一 项与 X k 1 k 中的每一项都不相关 X k 1 k 1 X k 1 X k 1 k 1 X k 1 k X k 1 k X k 1 k Kg k 1 Z k 1 k X k 1 k Kg k 1 Z k 1 k X k 1 k Kg k 1 H X k 1 k v k 1 Kg k 1 v k 1 I Kg k 1 H X k 1 k 我们已经知道 w k 1 与 v k 1 不相关 因此只要 X k 1 k 1 与上式的第二项 也不相关 就说明结论 b 成立 根据 a 中的结论 X k 1 k 的递归展开式中 出现的 v k w k v k 1 w k 1 等等 显然 w k 1 与 v m k k 1 都不相关 另外 由于 w k 1 的自相关为 函数 因此 w k 1 与 w m k k 1 也不相关 也就得出 w k 1 与 X k 1 k 不相关 进而可知 w k 1 与 X k 1 k 1 不相关 正是因为 a b 中的两个不相关 卡尔曼公式中的预测协方差矩 卡尔曼公式 2 和新息协方差矩阵 卡尔曼公式 3 中的 分母 部分 才可以是简单的加式 附注 2 卡尔曼滤波的五个公式 先验预测值与先验预测协方差矩阵的计算 求解协方差时 都认为预测值的期 望是实际值 因此 X k 1 k 的协方差矩阵同样也是 X k 1 k 的协方差矩阵 又因为偏差 X k 1 k 的期望是 0 因此协方差矩阵反映了 X k 1 k 在向量空间 中的模长 注意 协方差矩阵都是对称矩阵 X k 1 k A X k k B U k 1 P k 1 k A P k k A Q k 2 卡尔曼增益矩阵的计算 量测预测值为 Z k 1 k H X k 1 k 新息协方差见公式 3 中的 分母 部分 量测预测值的期望是实际量测值 因 此 Z k 1 k 的协方差矩阵同样也是 Z k 1 k 的协方差矩阵 又因为偏差 Z k 1 k 的期望是 0 因此协方差矩阵反映了 Z k 1 k 在向量空间中的模长 Kg k 1 P k 1 k H H P k 1 k H R k 1 3 后验预测值与后验预测协方差矩阵的计算 X k 1 k 1 X k 1 k Kg k 1 Z k 1 H X k 1 k 4 P k 1 k 1 I Kg k 1 H P k 1 k 5 clear 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 b t c p1 t c 2 p1 t Rvv s t a s t 1 b t Y t a c s t 1 p t p1 t c b t p1 t end t 1 N plot t s r t Y g t x b Kalman 滤波器 A 1 0 1 1 0 0 4 9 2 B 6 1 1 C 0 0 1 D 0 S ss A B C D Q 0 001 R 0 1 kest L P kalman S Q R 运行程序 得到系统 Kalman 滤波器的增益矩阵 L 与估计误差的协 方差 P 为 L 1 0641 1 1566 2 0393 P 0 0678 0 0664 0 1064 0 0664 0 0695 0 1157 0 1064 0 1157 0 2039 卡尔曼滤波算法及 MATLAB 实现 2012 08 29 21 39 56 转 载 这一段时间对现代滤波进行了学习 对自适应滤波器和卡尔曼滤波器有了一定认识 并对它们用 MATLAB 对语音信号进行了滤波 发现卡尔曼滤波器还是比较有用 能够在较大 的噪声中还原原来的信号 新的学期马上就开始了 由于 TI 的开发板一直在维修 所以学 习 TI 开发板的计划搁置 但是对声音信号的处理及滤波器的认识有了进一步提高 新的学 期继续努力 卡尔曼滤波的基本思想是 以最小均方误差为最佳估计准则 采用信号与噪声的状态 空间模型 利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计 求出当 前时刻的估计值 算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均 方误差的估计 语音信号在较长时间内是非平稳的 但在较短的时间内的一阶统计量和二阶统计量近似为 常量 因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳 态输出 假定语音信号可看成由一 AR 模型产生 时间更新方程 测量更新方程 K t 为卡尔曼增益 其计算公式为 其中 分别为过程模型噪声协方差和测量模型噪声协方差 测量协方差可以通过观测得到 则 较难确定 在本实验中则通过与两者比较得到 由于语音信号短时平稳 因此在进行卡尔曼滤波之前对信号进行分帧加窗操作 在滤 波之后对处理得到的信号进行合帧 这里选取帧长为 256 而帧重叠个数为 128 下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号 原声音信号与加噪声后的信号 原声音信号与经卡尔曼滤波处理后