INS/GNSS组合导航（〇）卡尔曼滤波直观理解

scott198512

已于 2023-05-02 22:09:02 修改

阅读量3.3k

点赞数 8

分类专栏： INS/GNSS组合导航文章标签：机器人

于 2022-09-16 17:17:45 首次发布

原文链接：https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

版权

INS/GNSS组合导航专栏收录该内容

18 篇文章 150 订阅

订阅专栏

"If you can't explain it simply, you don't understand it well enough."

Albert Einstein

卡尔曼滤波是非常经典的预测追踪算法，能够在系统存在噪声和干扰的情况下进行系统状态的最优估计，广泛使用在导航、制导、控制相关的领域包括航空、航天、航海、汽车导航与定位等。

对于某个动态系统有不确定信息的任何地方都可以使用卡尔曼滤波器，它能根据系统当前状态对系统接下来的状态信息做出有根据的预测。

卡尔曼滤波器非常适合不断变化的系统。它的优点是存储需求少，不需要保留除前一状态之外的更多历史记录，且处理速度非常快，非常适用于实时预测问题和嵌入式系统。

完全从数学角度推导卡尔曼滤波器的实现，相应的表达式繁琐而复杂，且理解起来不够直观。本篇通过一个简单的例子，并辅之以清晰简洁的图片展示了卡尔曼滤波器实现的精要部分。

1.一个简单的例子

假设我们开发了一台无人机（假设它的名字是Eva），想要用它来在城市中送快递，Eva身上有一些传感器，可以让我们知道它的速度v(比如三维空间中沿x,y,z各轴的速度大小)，同时Eva身上还有GPS系统、气压计等设备，可以获知它的位置p(比如经纬度，海拔高度等)，也就是说我们可以实时观测Eva的状态。

那么我们可以把Eva的某一个时刻的状态表示为一个关于位置与速度的向量：

$\large \vec{x} =\begin{bmatrix}{p} \\ \\ {v} \end{bmatrix}$

状态也可以是其它物理量，比如它可以是油箱中的液位、汽车发动机的温度、用户手指在触摸板上的位置或需要跟踪的任何数据。在我们的示例中，它是位置和速度。

2.不确定性和相关性表示

虽然我们比较肯定Eva此时的状态，但是系统无论如何总是会存在误差的，无论是计算上，还是传感器的测量上，所以我们只能认为当前状态是当前真实状态的一个最优估计。那么我们不妨认为Eva的当前状态服从一个高斯分布，如下图所示：

高斯分布的中心 $\large \mu$ 就是图中的 $\large \hat{x}_k$ ,即：

$\large \hat{x}_k=\begin{bmatrix}{position} \\ \\ {velocity} \end{bmatrix}$

对于方差 $\large \sigma ^2$ (也就是图中的椭圆的范围)，因为我们有两个变量，所以可以用一个协方差矩阵 $\large P_k$ 来表示:

$\large \begin{equation} \label{eq:statevars} \begin{aligned} & \mathbf{P}_k &= \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix} \end{aligned} \end{equation}$

协方差矩阵用于表示位置与速度的相关性，图示如下。

所以Eva的真实状态可能就位于上图椭圆的范围内，位于椭圆心的概率最大。上述两个物理量速度与位置显然属于有相关性的量，所以相应的协方差矩阵的非对角元素不为0。

3.预测下一个位置的系统状态与系统误差

接下来我们需要基于Eva当前的状态（k-1时刻），运用一些物理学的知识来预测它的下一个状态，通过简单的物理学知识，基于k-1时刻的位置和速度，可以推测下一个时刻的状态（k时刻）为：

表示为矩阵形式为：

此处的 $\large F_k$ 就是状态转移矩阵。 Eva的系统误差通过协方差矩阵 $\large P_k$ 来表示，根据协方差矩阵的性质：

那么我们所预测的Eva下一个时刻的状态误差为 $\large P_k$ ,结合起来就是：

图示表示上述过程为：

4.考虑系统内部控制

进一步地，外部世界与上述系统状态无关的变化可能会影响系统的状态。

例如，如果状态模拟火车的运动，火车操作员可能会踩油门，导致火车加速。同样，在我们的机器人示例中，导航软件可能会发出命令来转动车轮或停止。如果我们知道这个关于世界上正在发生的事情的额外信息，我们可以把它塞进一个向量用 $\large \vec{u}_k$ 表示，对其进行处理，并将其添加到我们的预测中作为修正。

假设我们知道机器人预期的加速度为 $\large a$ ，从基本运动学我们得到：

矩阵表示为：

新方程中的 $\large \mathbf{B}_k$ 我们称之为状态控制矩阵，而 $\large \color{DarkOrange}{\vec{\mathbf{u}}}_k$ 称之为状态控制向量，含义很明显，前者表明的是加速或减速如何改变Eva的状态，而后者则表明控制的力度大小和方向。

5.外部影响导致的不确定性

如果状态完全基于其自身属性而演变，那么一切都很好。但如果系统状态还受到外部因素的影响，那么我们就需要知道那些外部影响因素到底是什么。

比如机器人受到顺风或逆风或地面上的颠簸、摩擦打滑可能会影响它的速度。我们无法一一跟踪这些影响，如果发生此类情况，我们的预测就会出错，因为我们没有考虑到外在影响。

我们可以通过在每个预测步骤之后添加一些新的不确定性来模拟与“世界”相关的不确定性（即我们没有跟踪到的影响因素），图示表示如下：

我们先前估计的每个状态 $\large x_{k-1}$ 都可能转移到一系列状态(一个高斯分布的区域),而不再是唯一的 $\large x_k$ 。因为使用高斯分布建模最具有普适性，所以我们假定每个点 $\large \vec{x}_{k - 1}$ 被转移到具有协方差 $\large Q_k$ 的高斯分布内，换言之，我们将未跟踪的影响视为协方差噪声矩阵 $\large Q_k$ 。

这会产生一个新的高斯分布，具有不同的协方差（与先前未添加外部影响时的均值相同），也就是下文提到的扩展协方差，图示表示为：

我们通过简单地添加 $\large Q_k$ 得到扩展的协方差矩阵，从而给出预测步骤（prediction step）的完整表达式如下：

换句话说，新的最优估计 $\large \hat{x}_{k}$ 是根据先前的最优估计做出的预测，加上对已知外部影响的修正。

新的不确定性 $\large P_k$ 是从旧的不确定性中预测出来的，另外还有一些来自环境的不确定性。

6.实际观测改进估计

6.1预期的结果

如果我们有几个传感器比如一个读取位置信息，另一个读取速度信息。每个传感器提供一些关于状态的间接信息，也就是说，原始估计的每个状态可能和传感器多个读数对应起来（一定范围内一对多）。

注意，上述读数的单位和比例可能与我们正在跟踪的状态的单位和比例不同，那么我们将使用矩阵 $\large H_k$ 对传感器进行建模，图示表示如下：

我们可以计算出我们所期望看到的传感器读数的分布：

因此我们就完成了对观测值的预测，预测其结果服从如下高斯分布：

6.2实际的观测结果

由于我们的传感器至少有些测量误差，我们先前估计的每个状态都可能导致多个传感器读数。

从我们观察到的每个读数中，我们可能会猜测我们的系统处于特定状态。但实际上由于存在不确定性的影响，能够产生我们看到读数的状态可能并不是真实状态（可能其它的状态更合适）。

我们将这种不确定性即传感器噪声协方差矩阵表示为 $\large R_k$ , 对应的概率分布的均值即我们观察到的读数，我们称之为 $\large \vec{z}_k$ 。

现在我们有两个高斯分布：一个叠加到变换后的预测的平均值上，表现为过程噪声 $\large Q_k$ ，一个叠加到的实际传感器读数上，表现为测量噪声 $\large R_k$ 。

基于此，对基于预测状态（粉红色）所对应的读数，以及基于我们实际观察到的传感器读数（绿色），我们必须融合二者的结果（实际上是通过融合预测数据与观测数据，对误差进行闭环管理，将误差限定在一定范围，防止误差随时间的累积越来越大），从而给出最可能的新状态估计。

由此，对于任何读数 $\large (z_1,z_2)$ ，我们有两个相关的概率分布：

(1) 在 $\large \large (z_1,z_2)$ 读数下，传感器测量读数 $\large \vec z_k$ 是正确或错误的概率（测量值对应的概率）；

（2）基于先前状态估计认为 $\large (z_1,z_2)$ 是我们应该看到的读数对应的概率（预测值对应的概率）。

如果我们想知道两种情况都满足的概率，那么我们只需将它们相乘。因此，我们将两个高斯分布相乘，得到的结果图示表示如下：

基于两个高斯分布相乘，我们就得到另一个高斯分布，具有新的均值与协方差。

7.高斯分布的乘积

具有均值与方差分别为 $\large \mu,\sigma^2$ 的一维高斯分布的表达式如下：

两个高斯分布（红色与绿色曲线）乘积后得到未归一化的蓝色曲线（两者的交集）：

经过一系列的推导可得新的高斯分布的均值与方差（详细推导可见文末参考文献链接3 ）。

上式中可以提取一小部分表示为比例项 $\large k$

由此得到

可参考如下卡尔曼滤波视频

根据 $\mu',\sigma'$ 的表达式可知，直观上， $0<k<1$ ,表现出如下趋势

a) $while \quad \sigma_0^2 <<\sigma_1^2, k\rightarrow 0, \mu'\rightarrow \mu_0,\sigma'^2\rightarrow \sigma_0^2$

b) $while \quad \sigma_1^2 <<\sigma_0^2, k\rightarrow 1, \mu'\rightarrow \mu_1,\sigma'^2\rightarrow 0$

将它们写成矩阵形式，也就是对应的高斯分布不再是简单的一维，而是为二维或高维：

前面我们已经得到了预测结果和观测结果服从的两个高斯分布，如下：

结合前两组方程，推导卡尔曼滤波对当前状态(基于预测和观测的)最优估计的计算方程：

两边化简，注意K可以展开，经过左乘和右乘相关矩阵和逆，于是可以得到

其中 $\large { K}'$ 就是传说中的卡尔曼增益，即：

这样就实现了完整的更新步骤（update step）， $\large \hat{ {x}}'_k$ 就是我们新的最优估计，然后就可以开始下一轮的预测与更新，整个迭代过程的完整流程图如下：

8.总结与展望

1.预测（prediction step）

2.更新（update step）

通常也写作如下形式：

或者如下形式：

也有文献有如下不同的写法，相应的对应关系如下

上面的 $\large y_k$ 是测量余量(measurement residual)， $\large S_k$ 是测量余量协方差矩阵。

上述完整的KF可以准确地建模任何线性系统。对于非线性系统，可以使用扩展卡尔曼滤波器EKF，它通过简单地对预测值与测量值的均值进行线性化处理，此处不再详述。

9.实例推导

实际假定只有两个方向 $x,y$ 的位置与速度（根据实际需要可考虑添加 $z$ 方向），位置与速度组成的状态向量如下

$X=[p^T,v^T]^T$

其中位置状态 $p=[p_x,p_y]^T$ ，速度 $v=[v_x,v_y]^T$ , $k$ 时刻的状态由前一时刻 $k-1$ 进行预测。

$X_k=\begin{bmatrix}p_k \\v_k \end{bmatrix}= \begin{bmatrix}p_{k-1}+v_{k-1}\Delta t+ \frac{1}{2}\tilde{a}_{k-1}\Delta t^2 \\v_{k-1}+\tilde{a}_{k-1}\Delta t \end{bmatrix}$

展开上式得到

$X_k= \begin{bmatrix}p_{x,k-1}+v_{x,k-1}\Delta t+ \frac{1}{2}\tilde{a}_{x,k-1}\Delta t^2 \\ \\p_{y,k-1}+v_{y,k-1}\Delta t+ \frac{1}{2}\tilde{a}_{y,k-1}\Delta t^2\\\\v_{x,k-1}+\tilde{a}_{x,k-1}\Delta t \\\\v_{y,k-1}+\tilde{a}_{y,k-1}\Delta t \end{bmatrix}$

其中， $\tilde{a}_{x},\tilde{a}_{y}$ 应的是x,y方向的加速度，上式整理得到如下矩阵表示形式：

$X_k= \begin{bmatrix}I_{2\times2} & I_{2\times2}\Delta t \\ \\0_{2\times2}&I_{2\times2} \end{bmatrix}X_{k-1}+\begin{bmatrix}\frac{1}{2}I_{2\times2}\Delta t^2 \\ \\I_{2\times2}\Delta t\end{bmatrix}\begin{bmatrix}\tilde{a}_{x,k-1} \\\\\tilde{a}_{y,k-1}\end{bmatrix}$

过程噪声来源于加速度的输出，也就是 $\tilde{a}_{k-1}={a}_{k-1}+e_{k-1}$ ,展开得到:

$\tilde{a}_{x,k-1}={a}_{x,k-1}+e_{x,k-1}$

$\tilde{a}_{y,k-1}={a}_{y,k-1}+e_{y,k-1}$

其中， $e_{k-1}$ 表示加速度的输出噪声，假定其服从0均值高斯分布 $e_{k-1} \sim N(0,I_{2\times 2} \sigma_e^2)$

根据协方差关系式 $Cov(Ax)=A \Sigma A^T$ ，其中 $Cov(x)=\Sigma$ ，由此得到如下过程噪声协方差矩阵

$Q=\begin{bmatrix}\frac{1}{2}I_{2\times2}\Delta t^2 \\ \\I_{2\times2}\Delta t\end{bmatrix}I_{2 \times 2} \sigma_e^2 \begin{bmatrix}\frac{1}{2}I_{2\times2}\Delta t^2 \\ \\I_{2\times2}\Delta t\end{bmatrix}^T=\begin{bmatrix}\frac{1}{2}I_{2\times2}\Delta t^2 \\ \\I_{2\times2}\Delta t\end{bmatrix} \begin{bmatrix} \sigma_{ax}^2&0 \\ 0 &\sigma_{ay}^2 \end{bmatrix} \begin{bmatrix}\frac{1}{2}I_{2\times2}\Delta t^2 \\ \\I_{2\times2}\Delta t\end{bmatrix}^T$

展开得到过程噪声协方差矩阵完整形式如下：

$Q= \begin{bmatrix}\frac{1}{4}\Delta t^4 \sigma_{ax}^2 &0 & \frac{1}{2}\Delta t^3 \sigma_{ax}^2 & 0 \\ \\ 0 &\frac{1}{4}\Delta t^4 \sigma_{ay}^2 & 0 & \frac{1}{2}\Delta t^3 \sigma_{ay}^2\\\\ \frac{1}{2}\Delta t^3 \sigma_{ax}^2 &0 & \Delta t^2 \sigma_{ax}^2 & 0 \\\\ 0 &\frac{1}{2}\Delta t^3 \sigma_{ay}^2 & 0 & \Delta t^2 \sigma_{ay}^2 \end{bmatrix}$

通常情况下，可假定两个方向的噪声方差大小一致， $\sigma_{ax}^2=\sigma_{ay}^2=\sigma_e^2$ ，上式简化为

$Q= \begin{bmatrix}\frac{1}{4}\Delta t^4 \sigma_{e}^2 &0 & \frac{1}{2}\Delta t^3 \sigma_{e}^2 & 0 \\ \\ 0 &\frac{1}{4}\Delta t^4 \sigma_{e}^2 & 0 & \frac{1}{2}\Delta t^3 \sigma_{e}^2\\\\ \frac{1}{2}\Delta t^3 \sigma_{e}^2 &0 & \Delta t^2 \sigma_{e}^2 & 0 \\\\ 0 &\frac{1}{2}\Delta t^3 \sigma_{e}^2 & 0 & \Delta t^2 \sigma_{e}^2 \end{bmatrix}$

由此得到过程模型如下：

$X_k=AX_{k-1}+Ba_{k-1}+w_{k-1}$

其中

$A= \begin{bmatrix}I_{2\times2} & I_{2\times2}\Delta t \\ \\0_{2\times2}&I_{2\times2} \end{bmatrix} = \begin{bmatrix}1 &0 & \Delta t & 0 \\ \\ 0 &1 & 0 & \Delta t\\\\ 0 &0 & 1 & 0 \\\\ 0 &0 & 0 & 1 \end{bmatrix}$

$B=\begin{bmatrix}\frac{1}{2}I_{2\times2}\Delta t^2 \\ \\I_{2\times2}\Delta t\end{bmatrix}= \begin{bmatrix}\frac{1}{2}\Delta t^2 &0 \\ \\ 0 & \frac{1}{2}\Delta t^2\\\\ \Delta t &0 \\\\ 0 &\Delta t \end{bmatrix}$