卡尔曼滤波vb.net 卡尔曼滤波原理及应用matlab仿真

kalman滤波原理

卡尔曼(kalman)滤波 卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全包含噪声的测量(英文: measurement)中，估计动态系统的状态。 应用实例 卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的， 包含噪声的观察序列预测出物体的坐标位置及速度. 在很多工程应用(雷达, 计算机视觉)中都可以找到它的身影. 同时， 卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题. 比如,在雷达中,人们感兴趣的是跟踪目标,但目标的位置,速度, 加速度的测量值往往在任何时候都有噪声. 卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响, 得到一个关于目标位置的好的估计。 这个估计可以是对当前目标位置的估计(滤波), 也可以是对于将来位置的估计(预测)， 也可以是对过去位置的估计(插值或平滑). 命名 这种滤波方法以它的发明者鲁道夫.E.卡尔曼（Rudolf E. Kalman）命名. 虽然Peter Swerling实际上更早提出了一种类似的算法. 斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器. 卡尔曼在NASA埃姆斯研究中心访问时, 发现他的方法对于解决阿波罗计划的轨道预测很有用, 后来阿波罗飞船的导航电脑使用了这种滤波器. 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表. 目前,卡尔曼滤波已经有很多不同的实现. 卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器.除此以外, 还有施密特扩展滤波器,信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种.也行最常见的卡尔曼滤波器是锁相环， 它在收音机,计算机和几乎任何视频或通讯设备中广泛存在.

专注于为中小企业提供做网站、网站建设服务,电脑端+手机端+微信端的三站合一,更高效的管理,为中小企业新洲免费做网站提供优质的服务。我们立足成都，凝聚了一批互联网行业人才，有力地推动了千余家企业的稳健成长，帮助中小企业通过网站建设实现规模扩充和转变。

卡尔曼滤波的基本原理和算法

卡尔曼滤波的原理用几何方法来解释。这时，~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列的矩阵。

参考资料：

详解卡尔曼滤波（Kalman Filter）原理

看过很多关于卡尔曼滤波的资料，发现很多资料写的都很晦涩，初学者很难看懂。在网上找了很多资料之后，发现了 这篇博文 讲的非常清晰易懂，特此翻译记录，以备后用。为保证翻译的原滋原味，以下均用第一人称表述。

我不得不说一说卡尔曼滤波，因为它能做到的事情简直令人惊叹。

很可惜的是，很少有软件工程师和科学家对此有深入了解。这让我感到很沮丧，因为卡尔曼滤波是如此通用且强大的工具，它能在不确定情况下 组合信息 。有时，它提取准确信息的能力似乎几乎是不可思议的。如果听起来我讲的太多了，那么请看一下之前 发布的视频 ，其中演示了一个利用卡尔曼滤波观察自由浮动物体的速度来确定它的方向。真棒！

你可以在任何含有 不确定信息 的动态系统中的使用卡尔曼滤波，对系统的下一步动作做出 有根据的猜测 。即使伴随着各种干扰，卡尔曼滤波总是能指出真实世界发生的情况。它可以利用怪异现象之间的关联，你可能不会想到利用这些关联！

卡尔曼滤波对于持续变化的系统是理想的选择。由于卡尔曼滤波除了记忆前一个状态而不需要保留其他的历史记忆信息，因此卡尔曼滤波具有轻量化的特点，运行速度非常快，非常适合处理实时的问题和嵌入式系统。

你在Google上找到的大部分关于卡尔曼滤波的数学描述是晦涩难懂的。那是非常糟糕的状况！因为卡尔曼滤波能被简单和容易的方式所理解的。因此，本文是一个非常不错的文章主题，本文将尝试用许多清晰、美观的图片来阐明它。 本文的前提很简单，你仅仅需要对概率和矩阵有基本的了解。

本文将从一个简单的例子开始，说明卡尔曼滤波可以解决的问题。但如果你想直接接触精美的图片和数学，请随时跳转。

举一个简单的小例子：你已经做了一个能在丛林中游走的小机器人，为确保其能导航，机器人需要知道它所在的位置。

我们的机器人也有GPS传感器，精确大约10米，但它需要更精确地知道自己的位置。在树林中有很多沟壑和悬崖，如果机器人的误差超过几英尺，它可能会从悬崖上掉下去。所以仅依赖GPS进行定位是远远不够的。

我们可能还知道机器人是如何移动的：机器人知道发送给车轮马达的指令，如果它朝一个方向前进，没有任何干扰，下一刻它可能会继续朝同一方向前进。当然，它对自己的运动并不完全了解：它可能会受到风的冲击，车轮可能会打滑，或者在崎岖不平的地形上滚动；所以轮子转动的数量可能不能准确地代表机器人实际行走了多远，这个预测也不会完全准确。

GPS 传感器 告诉我们一些关于状态的信息，但只是间接的，带有一些不确定性而且并不精准。我们的 预测 告诉了机器人是如何移动的，但只是间接的，并且也是不确定和不精准的。

但是，如果我们利用所有可用的信息，我们能得到一个比这两个估计本身更好的答案吗?当然，答案是肯定的，这就是卡尔曼滤波器的作用。

让我们来看看我们想要诠释的一个场景。我们继续上一个例子，机器人仅仅包含一个位置和速度的简单状态。

更有趣的是下面的例子：位置和速度是呈 相关性 的。观察特定位置的可能性取决于你的速度：

这种关系非常重要，因为它给我们提供了更多的信息：一个测量值告诉我们其他测量值可能是什么。我们要尽可能多地从不确定的目标中压缩卡尔曼滤波器的信息！

这种相关性被称为 协方差矩阵 。简而言之，矩阵的每个元素

我们基于高斯分布来建立状态变量，所以在时间

接下来，我们需要某种方式来知道目前状态（时刻

对于上述所有的数学公式，你仅仅需要实现公式（7）、（18）和（19）。（如果你忘记了上述公式，你也能从公式（4）和（5）重新推导。）

这将允许你精确地建模任何线性系统。对于非线性系统，需要用到 扩展卡尔曼滤波 ，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

什么是卡尔曼滤波？

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

卡尔曼滤波

将预测值和测量值进行结合，对系统状态进行最优估计的算法。

在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

根据k-1时刻的系统状态预测k时刻系统状态。

考虑外部因素控制的影响

外部因素会对系统进行控制，从而带来一些与系统自身状态没有相关性的改变。其中 成为控制矩阵， 称为控制向量，如果没有外部控制，这部分可以忽略。

外部噪声因素

在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型

小结：

由上两式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。 而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

加入传感器观测数据

卡尔曼滤波的一大优点就是能处理传感器噪声，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。 从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。

传感器早上用协方差 表示，该分布的均值 是我们读取到的传感器数据。

于是我们得到两个高斯分布，一个是预测值附近，一个是传感器读数附近。把两个具有不同均值和方差的高斯分布相乘，得到一个新的具有独立均值和方差的高斯分布。

结果如下，其中，K为卡尔曼增益。

总结：

我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

参考文章：