详解卡尔曼滤波原理

什么是卡尔曼滤波？
我们能用卡尔曼滤波做什么？
卡尔曼滤波是如何看到你的问题的
使用矩阵来描述问题
外部控制量
外部干扰
用测量量来修正估计值
融合高斯分布
将所有公式整合起来
总结

什么是卡尔曼滤波？

你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。

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

我们能用卡尔曼滤波做什么？

用玩具举例：你开发了一个可以在树林里到处跑的小机器人，这个机器人需要知道它所在的确切位置才能导航。

我们可以说机器人有一个状态xk⃗\vec{x_k}xk，表示位置和速度：
x⃗k=(p⃗,v⃗)\vec{x}_k = (\vec{p}, \vec{v}) xk=(p,v)

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

这个机器人带有GPS，精度大约为10米，还算不错，但是，它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖，所以只有GPS是不够的。

或许我们知道一些机器人如何运动的信息：例如，机器人知道发送给电机的指令，知道自己是否在朝一个方向移动并且没有人干预，在下一个状态，机器人很可能朝着相同的方向移动。当然，机器人对自己的运动是一无所知的：它可能受到风吹的影响，轮子方向偏了一点，或者遇到不平的地面而翻倒。所以，轮子转过的长度并不能精确表示机器人实际行走的距离，预测也不是很完美。

GPS 传感器告诉了我们一些状态信息，我们的预测告诉了我们机器人会怎样运动，但都只是间接的，并且伴随着一些不确定和不准确性。但是，如果使用所有对我们可用的信息，我们能得到一个比任何依据自身估计更好的结果吗？回答当然是YES，这就是卡尔曼滤波的用处。

卡尔曼滤波是如何看到你的问题的

下面我们继续以只有位置和速度这两个状态的简单例子做解释。
x⃗=[pv]\vec{x} = \left[\begin{matrix}p\\ v \end{matrix}\right] x=[pv]

我们并不知道实际的位置和速度，它们之间有很多种可能正确的组合，但其中一些的可能性要大于其它部分：

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

在上图中，位置和速度是不相关的，这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣：位置和速度是相关的，观测特定位置的可能性取决于当前的速度：

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

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

使用矩阵来描述问题

我们基于高斯分布来建立状态变量，所以在时刻kkk需要两个信息：最佳估计x^k\hat{x}_kx^k（即均值，其它地方常用 μ\muμ 表示），以及协方差矩阵PkP_kPk。
x^k=[positionvelocity],Pk=[ΣppΣpvΣvpΣvv]\hat{x}_k=\left[\begin{matrix}position\\ velocity \end{matrix}\right], \ \ \ \ P_k=\left[\begin{matrix} \Sigma_{pp} & \Sigma_{pv}\\ \Sigma_{vp} & \Sigma_{vv} \end{matrix}\right] x^k=[positionvelocity], Pk=[ΣppΣvpΣpvΣvv]

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

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

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：
pk=pk−1+Δtvk−1vk=vk−1p_k = p_{k-1}+\Delta t v_{k-1}\\ v_k = \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ v_{k-1} pk=pk−1+Δtvk−1vk= vk−1
即，
x^k=[1Δt01]x^k−1=Fkx^k−1\hat{x}_k=\left[\begin{matrix} 1 & \Delta t\\ 0 & 1 \end{matrix}\right]\hat{x}_{k-1} =F_k \hat{x}_{k-1} x^k=[10Δt1]x^k−1=Fkx^k−1

现在，我们有了一个预测矩阵来表示下一时刻的状态，但是，我们仍然不知道怎么更新协方差矩阵。此时，我们需要引入另一个公式，如果我们将分布中的每个点都乘以矩阵 AAA，那么它的协方差矩阵Σ\SigmaΣ会怎样变化呢？很简单，下面给出公式：
Cov(x)=ΣCov(Ax)=AΣATCov(x)=\Sigma\\ Cov(Ax)=A\Sigma A^T Cov(x)=ΣCov(Ax)=AΣAT
综合以上公式，可得，
x^k=Fkx^k−1Pk=FkPk−1FkT\hat{x}_k=F_k \hat{x}_{k-1}\\ P_k = F_k P_{k-1}F_k^T x^k=Fkx^k−1Pk=FkPk−1FkT

外部控制量

我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。

以火车的运动状态模型为例，火车司机可能会操纵油门，让火车加速。相同地，在我们机器人这个例子中，导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息，我们可以用一个向量u⃗k\vec{u}_kuk来表示，将它加到我们的预测方程中做修正。

假设由于油门的设置或控制命令，我们知道了期望的加速度aaa，根据基本的运动学方程可以得到：
pk=pk−1+Δtvk−1+12aΔt2vk=vk−1+aΔtp_k = p_{k-1} + \Delta t v_{k-1} + \frac{1}{2}a \Delta t^2\\ v_k = v_{k-1} + a \Delta t pk=pk−1+Δtvk−1+21aΔt2vk=vk−1+aΔt

以矩阵的形式表示就是：
x^k=Fkx^k−1+[Δt22Δt]=Fkx^k−1+Bku⃗k\hat{x}_k = F_k \hat{x}_{k-1} + \left[\begin{matrix} \frac{\Delta t^2}{2}\\ \Delta t \end{matrix}\right] = F_k\hat{x}_{k-1}+B_k \vec{u}_k x^k=Fkx^k−1+[2Δt2Δt]=Fkx^k−1+Bkuk

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

外部干扰

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。

但是，如果存在未知的干扰呢？例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。

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

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

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

我们通过简单地添加QkQ_kQk得到扩展的协方差，下面给出预测步骤的完整表达式：
x^k=Fkx^k−1+Bku⃗kPk=FkPk−1FkT+Qk\hat{x}_k = F_k \hat{x}_{k-1} + B_k \vec{u}_k\\ \ \\ P_k = F_k P_{k-1}F_k^T + Q_k x^k=Fkx^k−1+Bkuk Pk=FkPk−1FkT+Qk

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

好了，我们对系统可能的动向有了一个模糊的估计，用x^k\hat{x}_kx^k和PkP_kPk来表示。如果再结合传感器的数据会怎样呢？

用测量量来修正估计值

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

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

我们可以计算出传感器读数的分布，用之前的表示方法如下式所示：
μ⃗expected=Hkx^kΣexpected=HkPkHkT\vec{\mu}_{expected} = H_k \hat{x}_k\\ \ \\ \Sigma_{expected} = H_kP_kH_k^T μexpected=Hkx^k Σexpected=HkPkHkT

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

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

我们将这种不确定性（例如：传感器噪声）用协方差RkR_kRk表示，该分布的均值就是我们读取到的传感器数据，称之为z⃗k\vec{z}_kzk。

现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

我们必须在预测值（粉红色）和传感器测量值（绿色）之间找到最优解。

那么，我们最有可能的状态是什么呢？对于任何可能的读数(z1,z2)(z_1, z_2)(z1,z2)，有两种情况：（1）传感器的测量值；（2）由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。

剩下的就是重叠部分了，这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。

瞧！这个重叠的区域看起来像另一个高斯分布。　

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

融合高斯分布

先以一维高斯分布来分析比较简单点，具有方差σ2\sigma^2σ2 和 μ\muμ的高斯曲线可以用下式表示：
N(x,μ,σ)=1σ2πe−(x−μ)22σ2N(x, \mu, \sigma) = \frac{1}{\sigma \sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}} N(x,μ,σ)=σ2π1e−2σ2(x−μ)2

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

将上式代入两个高斯分布的相乘中（注意重新归一化，使总概率为1），可以得到：
μ′=μ0+σ02(μ1−μ0)σ02+σ12σ′2=σ02−σ04σ02+σ12\mu\prime=\mu_0 + \frac{\sigma_0^2(\mu_1-\mu_0)}{\sigma_0^2+\sigma_1^2}\\ \ \\ \sigma\prime^2 = \sigma_0^2 - \frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2} μ′=μ0+σ02+σ12σ02(μ1−μ0) σ′2=σ02−σ02+σ12σ04

将上式中的两个式子相同的部分用kkk表示：
k=σ02σ02+σ12μ′=μ0+k(μ1−μ0)σ′2=σ02−kσ02k= \frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}\\ \ \\ \mu\prime=\mu_0 +k(\mu_1-\mu_0)\\ \ \\ \sigma\prime^2 = \sigma_0^2 -k\sigma_0^2 k=σ02+σ12σ02 μ′=μ0+k(μ1−μ0) σ′2=σ02−kσ02

下面进一步将上面两个式子写成矩阵的形式，如果Σ\SigmaΣ表示高斯分布的协方差，μ⃗\vec{\mu}μ表示每个维度的均值，则：
K=Σ0(Σ0+Σ1)−1μ⃗′=μ⃗0+K(μ⃗1−μ⃗0)Σ′=Σ0−KΣ0K = \Sigma_0(\Sigma_0+\Sigma_1)^{-1}\\ \ \\ \vec{\mu}\prime = \vec{\mu}_0 + K( \vec{\mu}_1 - \vec{\mu}_0)\\ \ \\ \Sigma\prime = \Sigma_0 - K\Sigma_0 K=Σ0(Σ0+Σ1)−1 μ′=μ0+K(μ1−μ0) Σ′=Σ0−KΣ0

矩阵KKK称为卡尔曼增益。

将所有公式整合起来

我们有两个高斯分布，预测部分(μ0,Σ0)=(Hkx^k,HkPkHkT)(\mu_0, \Sigma_0) = (H_k\hat{x}_k, H_kP_kH_k^T)(μ0,Σ0)=(Hkx^k,HkPkHkT), 和测量部分(μ1,Σ1)=(z⃗k,Rk)(\mu_1, \Sigma_1) = (\vec{z}_k, R_k)(μ1,Σ1)=(zk,Rk)，将他们放到上式中算出它们之间的重叠部分：
Hkx^k′=Hkx^k+K(z⃗k−Hkx^k)HkPk′HkT=HkPkHkT−KHkPKHKTH_k\hat{x}_k\prime = H_k \hat{x}_k + K(\vec{z}_k-H_k\hat{x}_k)\\ \ \\ H_kP_k\prime H_k^T = H_kP_kH_k^T - KH_kP_KH_K^T Hkx^k′=Hkx^k+K(zk−Hkx^k) HkPk′HkT=HkPkHkT−KHkPKHKT
卡尔曼增益为：
K=HkPkHkT(HkPkHkT+Rk)−1K = H_kP_kH_k^T(H_kP_kH_k^T+R_k)^{-1} K=HkPkHkT(HkPkHkT+Rk)−1

将上面两式两边同时左乘或右乘矩阵的逆（注意：KKK里面包含了HkH_kHk）将其约掉，得到：
x^′=x^k+K′(z⃗k−Hkx^k)Pk′=Pk−K′HkPkK′=PkHkT(HkPkHkT+Rk)−1\hat{x}\prime = \hat{x}_k + K\prime(\vec{z}_k - H_k\hat{x}_k)\\ \ \\ P_k\prime = P_k - K\prime H_k P_k\\ \ \\ K\prime = P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} x^′=x^k+K′(zk−Hkx^k) Pk′=Pk−K′HkPk K′=PkHkT(HkPkHkT+Rk)−1

上面得到的即为完整的更新步骤方程。其中，x^k′\hat{x}_k\primex^k′就是新的最优估计，我们可以将它和Pk′P_k\primePk′放到下一个预测与更新方程中不断迭代。

总结

根据以上推导，我们得到了卡尔曼的预测和更新步骤，其中
预测步骤为：
x^k=Fkx^k−1+Bku⃗kPk=FkPk−1FkT+Qk\hat{x}_k = F_k \hat{x}_{k-1} + B_k \vec{u}_k\\ \ \\ P_k = F_k P_{k-1}F_k^T + Q_k x^k=Fkx^k−1+Bkuk Pk=FkPk−1FkT+Qk
更新步骤为：
x^′=x^k+K′(z⃗k−Hkx^k)Pk′=Pk−K′HkPkK′=PkHkT(HkPkHkT+Rk)−1\hat{x}\prime = \hat{x}_k + K\prime(\vec{z}_k - H_k\hat{x}_k)\\ \ \\ P_k\prime = P_k - K\prime H_k P_k\\ \ \\ K\prime = P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} x^′=x^k+K′(zk−Hkx^k) Pk′=Pk−K′HkPk K′=PkHkT(HkPkHkT+Rk)−1

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

自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理相关推荐

自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍
KF&EKF&UKF&PF 1. 基础知识概要协方差矩阵多维高斯分布状态空间表达式 2. 什么是卡尔曼滤波器 3. 五个重要的公式公式介绍公式推导过程 4. 卡尔曼滤 ...
自动驾驶算法-滤波器系列（八）——IMM交互多模型介绍
IMM交互多模型介绍 1. 简介 (1)IMM(Interacting Multiple Model) (2)马尔科夫概率转移矩阵 2. 算法流程 (1)输入交互(模型j) (2)卡尔曼滤波(模型j) ...
自动驾驶算法-滤波器系列（七）——ESKF(error-state Kalman Filter)介绍
IMM(Interacting Multiple Model) 1. ESKF是什么? 2. ESKF如何演变出来的? 3. ESKF主要解决什么问题? 4. ESKF算法原理 5. 总结 1. ES ...
自动驾驶算法-滤波器系列（六）——10+种经典滤波算法
经典滤波器介绍 1.限幅滤波法(又称程序判断滤波法) 2.中位值滤波法 3.算术平均滤波法 4.递推平均滤波法(又称滑动平均滤波法) 5.中位值平均滤波法(又称防脉冲干扰平均滤波法) 6.限幅平均滤波 ...
自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用
CTRV_UKF, CTRA_UKF 1. 基于CTRV的UKF 1.1 CTRV模型 1.2 UKF模型 2. 基于CTRA的UKF 2.1 CTRA模型扩展卡尔曼滤波算法是对非线性的系统方程或者 ...
自动驾驶算法-滤波器系列（四）——不同运动模型在KF/EKF中的应用
不同运动模型应用于KF/EKF 1. 基于匀速(CV)运动模型的KF 2. 基于匀加速(CA)运动模型的KF 3. 基于CTRV的EKF 4. 基于CTRA的EKF 上一篇文章中,笔者针对四种不同的运 ...
自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导
CV & CA & CTRV & CTRA 0. 运动模型简介 1. CV模型 2. CA模型 3. CTRV模型 4. CTRA模型上一篇文章主要讲解了不同卡尔曼滤波的原理 ...
一文透彻详解卡尔曼滤波原理
文章目录前言一卡尔曼滤波中的五个公式二重要公式与变量的理解三卡尔曼滤波的一个简单demo 前言由于最近一段时间在做多传感器融合工作,以下是根据这段时间的实践与学习,对卡尔曼滤波的一些理 ...
自动驾驶算法详解(5): 贝塞尔曲线进行路径规划的python实现
一.理论知识 1.路径规划定义路径规划智能物流.无人驾驶等智能领域中重要的组成部分.路径规划的目标是实现从目的地到终点之间寻找一条安全(无碰撞).高效(最短距离或最短时间)的一条最优或接近最优的路 ...

自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理