有一条曲折的小径，通向一棵大树。提出了一个要求：沿着小径从起点到树下。

很简单。 A所以他沿着小径走到树下。

现在，难度增加了：蒙上眼睛。

也不难，我当过特种兵。 B于是他歪歪扭扭地走到树旁。唉，好久不练，生疏。 (仅凭自己的预测能力)

看我的，我有 DIY 的 GPS！” C于是他像醉汉一样歪歪扭扭地走到树旁。唉，这个 GPS 没有做好，漂移太大。(仅依靠外界大噪音的测量)

我来试试。 旁边一个也当过特种兵 GPS，蒙上眼睛，沿着小径顺利地走到树下。(我可以预测自己 测量结果的反馈)

这么厉害！你是谁？
“卡尔曼 ！”
卡尔曼？！你是卡尔曼吗？大家都很惊讶。
这是我说的 GPS 卡而慢。”

以玩具为例:你开发了一个可以在树林里跑来跑去的小机器人，需要知道它的确切位置才能导航。

我们可以说，机器人有一状态，表示位置和速度：

请注意，这种状态只是一堆关于系统基本属性的数字，它可以是任何其他东西。在这个例子中，它是位置和速度，它也可以是容器中液体的总量，汽车发动机的温度，触摸板上用户手指的位置坐标，或任何你需要跟踪的信号。

有这个机器人GPS，精度在10米左右，还不错，但是需要把自己的位置准确到10米以内。森林里有许多沟壑和悬崖。如果机器人走错了一步，它可能会掉下悬崖，所以只有GPS是不够的。

也许我们知道一些关于机器人如何移动的信息：例如，机器人知道发送给电机的指令，是否向一个方向移动，没有人干预。在下一种状态下，机器人很可能朝同一个方向移动。当然，机器人对自己的运动一无所知：它可能受到风的影响，轮子的方向稍微偏离，或者在不平的地面上翻倒。因此，轮子的长度不能准确地表示机器人的实际行走距离，预测也不是很完美。

GPS 传感器告诉我们一些状态信息，我们预测它告诉我们机器人将如何运动，但它们只是间接的，并伴随着一些不确定性和不准确性。但是，如果我们使用所有可用的信息，我们能得到比根据自己估计更好的结果吗？当然，答案是YES，这就是卡尔曼滤波器的用途。

让我们继续解释一个简单的例子，只有位置和速度。
$$\vec{x}=[\begin{array}{c}p\\ v\end{array}]$$
我们并不知道实际的位置和速度，它们之间有很多种可能正确的组合，但其中一些的可能性要大于其它部分：

卡尔曼滤波假设两个变量（位置和速度，在这个例子中）都是随机的，并且服从高斯分布。每个变量都有一个均值$\mu$，表示随机分布的中心（最可能的状态），以及方差${\sigma }^2$，表示不确定性。

在上图中，位置和速度是不相关的，这意味着由其中一个变量的状态无法推测出另一个变量可能的值。

下面的例子更有趣：位置和速度是相关的，观测特定位置的可能性取决于当前的速度：

这种情况是有可能发生的，例如，我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。跟踪这种关系是非常重要的，因为它带给我们更多的信息：其中一个测量值告诉了我们其它变量可能的值，这就是卡尔曼滤波的目的，尽可能地在包含不确定性的测量数据中提取更多信息！

这种相关性用协方差矩阵来表示，简而言之，矩阵中的每个元素 ${\Sigma }_{ij}$ 表示第 $i$ 个和第$j$ 个状态变量之间的相关度。（你可能已经猜到协方差矩阵是一个对称矩阵，这意味着可以任意交换$i$ 和$j$）。协方差矩阵通常用 $\Sigma$ 来表示，其中的元素则表示为 ${\Sigma }_{ij}$ 。

我们基于高斯分布来建立状态变量，所以在时刻$k$需要两个信息：最佳估计${\hat{x}}_k$（即均值，其它地方常用$\mu$表示)，以及协方差矩阵 $P_k$ 。
$${\hat{x}}_k=\left[\begin{array}{c}position\\ velocity\end{array}\right]$$

$$P_k=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right](1)$$

当然，在这里我们只用到了位置和速度，实际上这个状态可以包含多个变量，代表任何你想表示的信息。接下来，我们需要根据当前状态（$k-1$时刻）来预测下一状态**（**$k$ 时刻）。记住，我们并不知道对下一状态的所有预测中哪个是“真实”的，但我们的预测函数并不在乎。它对所有的可能性进行预测，并给出新的高斯分布。

我们可以用矩阵$F_k$来表示这个预测过程：

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：

现在，我们有了一个预测矩阵来表示下一时刻的状态，但是，我们仍然不知道怎么更新协方差矩阵。此时，我们需要引入另一个公式，如果我们将分布中的每个点都乘以矩阵 A，那么它的协方差矩阵$\Sigma$会怎样变化呢？很简单，下面给出公式：

结合方程（4）和（3）得到：

我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。
　　以火车的运动状态模型为例，火车司机可能会操纵油门，让火车加速。相同地，在我们机器人这个例子中，导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息，我们可以用一个向量$\overrightarrow{u_k}$来表示，将它加到我们的预测方程中做修正。
　　假设由于油门的设置或控制命令，我们知道了期望的加速度 $a$ ，根据基本的运动学方程可以得到：

以矩阵的形式表示就是：

$B_k$称为控制矩阵，$\overrightarrow{u_k}$称为控制向量（对于没有外部控制的简单系统来说，这部分可以忽略)。让我们再思考一下，如果我们的预测并不是100%准确的，该怎么办呢？

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。
　　但是，如果存在未知的干扰呢？例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。
　　在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型：

原始估计中的每个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为。换句话说就是，我们将这些没有被跟踪的干扰当作协方差为的噪声来处理。

这产生了具有不同协方差（但是具有相同的均值）的新的高斯分布。

我们通过简单地添加得到扩展的协方差，下面给出预测步骤的完整表达式：

由上式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。
　　而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。
　　好了，我们对系统可能的动向有了一个模糊的估计，用和来表示。如果再结合传感器的数据会怎样呢？

我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。

注意，传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用矩阵$H_k$ 来表示传感器的数据。

我们可以计算出传感器读数的分布，用之前的表示方法如下式所示：

卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。

我们将这种不确定性（例如：传感器噪声）用协方差表示，该分布的均值就是我们读取到的传感器数据，称之为。
现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

我们必须在预测值（粉红色）和传感器测量值（绿色）之间找到最优解。
　　那么，我们最有可能的状态是什么呢？对于任何可能的读数，有两种情况：（1）传感器的测量值；（2)由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。

剩下的就是重叠部分了，这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。
　　瞧！这个重叠的区域看起来像另一个高斯分布。

如你所见，把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！下面用公式讲解。

先以一维高斯分布来分析比较简单点，具有方差 和 μ 的高斯曲线可以用下式表示：

如果把两个服从高斯分布的函数相乘会得到什么呢？

将式（9）代入到式（10）中（注意重新归一化，使总概率为1）可以得到：

将式（11）中的两个式子相同的部分用 k 表示：

下面进一步将式（12）和（13）写成矩阵的形式，如果 Σ 表示高斯分布的协方差，$\overrightarrow{u_k}$表示每个维度的均值，则：

矩阵称为卡尔曼增益，下面将会用到。放松！我们快要完成了！

我们有两个高斯分布，预测部分，和测量部分，将它们放到式（15)中算出它们之间的重叠部分：

由式（14）可得卡尔曼增益为：

将式（16）和式（17）的两边同时左乘矩阵的逆（注意里面包含了 $H_k$ ）将其约掉，再将式（16)的第二个等式两边同时右乘矩阵$H_k^T$ 的逆得到以下等式：

上式给出了完整的更新步骤方程。就是新的最优估计，我们可以将它和放到下一个预测和更新方程中不断迭代。

目的 ：对线性系统状态的估计

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

方法 ：利用状态过程中的噪声ω 和 观测噪声υ ，给予合理的权重，对状态x 进行估计。

先了解一下卡尔曼滤波的五个公式：

• 公式（一）： 计算预测状态值
• 公式（二）： 计算预测值和真实值之间的预测误差协方差矩阵
• 公式（三）： 在公式一和公式二的条件下，求得卡尔曼增益
• 公式（四）： 计算估计值
• 公式（五）： 计算估计值和真实值之间的误差协方差矩阵

在推导前，为了在推导过程中混淆，先做一个符号说明：

通过概率密度函数来定义高斯分布：

高斯分布的概率密度函数是：

均值为μ，标准差为σ 而上述的过程噪声ω 和 观测噪声υ皆服从高斯分布。

方差：各个样本与样本均值的差的平方和的均值，度量一组数据的分散程度。

协方差：两个变量总体误差的期望，度量两个变量线性相关性程度。 当两个变量是相同的情况，协方差就是方差。 如果两个变量的变化趋势相反，其中一个变量大于自身的期望值时，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。 两者之间的联系或者关系，关系越大，协方差越大。

协方差矩阵：数据集中两两变量的协方差组成，每个元素是各个向量元素之间的协方差 噪声协方差矩阵越小说明噪声的误差越小，可信度越高，其对角线上的值就是方差。 误差协方差矩阵越小说明过程噪声和量测噪声的关系越小，用比例分开过程噪声ω 和 观测噪声υ，如果关系越小，分开的越精确。比如一堆白砂糖和盐，如果两种混合的很均匀，说它关系很大，也就越难用比例的方法将其分开。

假定k时刻的系统状态与k-1时刻有关，并且内部有噪声，则

在无噪声情况下的预测

观测新息反应了预测噪声ω和观测噪声υ对状态的综合影响 将过程噪声ωk 看作一个 新息 * 比例，则

其中公式(6)，仅卡尔曼增益Kk为未知

来看误差协方差矩阵：而卡尔曼增益Kk应该使误差协方差矩阵Pk最小 状态新息（状态 - 预测状态）：

假设在无噪声情况下，状态新息:

则误差协方差矩阵：

此时，来看看预测误差协方差矩阵：

此时，再把误差协方差矩阵展开：

若使用T表示误差协方差矩阵的对角线，则

对T[Pk ]求导，就可找到最小的均方差，使T[Pk ]最小，就能得到卡尔曼增益Kk 对T[Pk ]求导：

令其为0，则卡尔曼增益：

其中观测矩阵H和观测噪声协方差矩阵R为常数，所以卡尔曼增益Kk与预测误差协方差矩阵Pk’ 有关

假设观测矩阵 H = 1，那么卡尔曼增益：

则Pk’ 越大，Kk越大，权重重视反馈测量 则Pk’ 越小，Kk越小，权重重视预测值 此时，来看看下一状态的预测误差协方差矩阵：

因为状态x 和过程噪声ωk是独立的，所以再简化

其中状态转移矩阵A和预测噪声协方差矩阵Q为常数，所以预测误差协方差矩阵Pk’与上一状态的误差协方差矩阵Pk-1有关

在加入外界对系统的作用时，可整理出卡尔曼滤波的五个公式：

用python来简单说明

import numpy as np
import matplotlib.pyplot as plt

delta_t = 0.1                               # 每秒钟采一次样
end_t = 7                                   # 时间长度
time_t = end_t * 10                         # 采样次数
t = np.arange(0, end_t, delta_t)            # 设置时间数组

u = 1                                       # 定义外界对系统的作用 加速度
x = 1 / 2 * u * t ** 2                      # 实际真实位置

v_var = 1                                   # 测量噪声的方差
# 创建高斯噪声，精确到小数点后两位
v_noise = np.round(np.random.normal(0, v_var, time_t), 2)

X = np.mat([[0], [0]])                      # 定义预测优化值的初始状态
v = np.mat(v_noise)                         # 定义测量噪声
z = x + v                                   # 定义测量值（假设测量值=实际状态值+噪声）
A = np.mat([[1, delta_t], [0, 1]])          # 定义状态转移矩阵
B = [[1 / 2 * (delta_t ** 2)], [delta_t]]   # 定义输入控制矩阵
P = np.mat([[1, 0], [0, 1]])                # 定义初始状态协方差矩阵
Q = np.mat([[0.001, 0], [0, 0.001]])        # 定义状态转移(预测噪声)协方差矩阵
H = np.mat([1, 0])                          # 定义观测矩阵
R = np.mat([1])                             # 定义观测噪声协方差
X_mat = np.zeros(time_t)                    # 初始化记录系统预测优化值的列表

for i in range(time_t):
    # 预测
    X_predict = A * X + np.dot(B, u)        # 估算状态变量
    P_predict = A * P * A.T + Q             # 估算状态误差协方差
    # 校正
    K = P_predict * H.T / (H * P_predict * H.T + R)     # 更新卡尔曼增益
    X = X_predict + K * (z[0, i] - H * X_predict)       # 更新预测优化值
    P = (np.eye(2) - K * H) * P_predict                 # 更新状态误差协方差
    # 记录系统的预测优化值
    X_mat[i] = X[0, 0]

plt.rcParams['font.sans-serif'] = ['SimHei']    # 设置正常显示中文
plt.plot(x, "b", label='实际状态值')             # 设置曲线数值
plt.plot(X_mat, "g", label='预测优化值')
plt.plot(z.T, "r--", label='测量值')
plt.xlabel("时间")                               # 设置X轴的名字
plt.ylabel("位移")                               # 设置Y轴的名字
plt.title("卡尔曼滤波示意图")                     # 设置标题
plt.legend()                                    # 设置图例
plt.show()                                      # 显示图表

运行上面的卡尔曼滤波，得到下图：

可以看出，尽管测量值波动很大，但最终的预测优化值与实际状态值相差不大。