卡尔曼滤波原理

卡尔曼滤波原理是指一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行估计的算法。

mpu6050卡尔曼滤波 mpu6050卡尔曼滤波算法mpu6050卡尔曼滤波 mpu6050卡尔曼滤波算法


mpu6050卡尔曼滤波 mpu6050卡尔曼滤波算法


由于观测数据中包括系统中的噪声和干扰的影响，所以估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术，卡尔曼滤波在测量方已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。

由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，卡尔曼滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。卡尔曼滤波不要求信号和噪声都是平稳过程的设条件。

对于每个时刻的系统扰动和观测误（即噪声），只要对它们的统计性质作某些适当的定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误为最小的真实信号的估计值。

因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

卡尔曼滤波

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

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

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

考虑外部因素控制的影响

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

外部噪声因素

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

小结：

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

加入传感器观测数据

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

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

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

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

总结：

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

参考文章：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

卡尔曼滤波的应用

卡尔曼滤波一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以估计也可看作是滤波过程。

应用：

在雷达中人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计，也可以是对于将来位置的预测，也可以是对过去位置的插值或平滑。