零基础读懂“扩展卡尔曼滤波”——下篇

作者: 非鱼知乐 | 来源:发表于2019-07-15 06:18 被阅读0次

https://mp.weixin.qq.com/s/__Lok-rPNz9eP7pKkR2nvQ

14. 一个简单的传感器融合实例

为了更好的理解传感器融合，我们将系统的状态限制为一个。为了进一步简化，假设我们不知道系统的状态转换模型 $A$ ，我们只知道传感器的值，假如使用两种不同的温度计测量室外的温度，缺少状态转换模型，我们假定温度计的当前状态与前一个状态是一样的，所以我们将状态转换矩阵设置为 1：

$\hat x_k=A\hat x_{k-1}=1\times \hat x_{k-1}=\hat x_{k-1}$

对于传感器融合，我们需要在观测向量中包含多个传感器，当然对于我们的例子，我们可以使用两个温度计的当前读数值，并且假定两个温度计对系统温度的贡献是一样的，那么我们就可以将观测矩阵 $C$ 设置为两个1：

$z_k=Cx_k+v_k= \left[ \begin{matrix} 1\\ 1 \end{matrix} \right] x_k+v_k$

现在我们已经有了三个矩阵 $A$ 、 $C$ 、 $R$ 中的两个 $A$ 、 $C$ ，那么如何得到 $R$ 呢？

回想一下在单个传感器中，我们将 $r$ 定义为观测噪声 $v_k$ 的方差，也就是在其平均值上下的变化量。那么对于一个多传感器的系统， $R$ 很显然应该是一个矩阵，包含每对传感器之间协方差的矩阵，这个矩阵对角线上的元素应该是每个传感器的 $r$ 值，即传感器自身的方差。对角线之外的元素应该表示两个传感器之间的噪声之间的关系，在许多实际例子中，两个不同的传感器之间的影响一般都设为 0，此处也一样，两个不同的温度计之间的噪声关系也应该为 0。

假设我们在恒温条件下观察了两个温度计，观察到它们的平均波动为 0.8°，也就是其读数标准差为 0.8，方差为 $0.8\times 0.8=0.64 。因此我们得出矩阵$ R$为：

$\left[ \begin{matrix} 0.64 & 0 \\ 0 & 0.64 \end{matrix} \right]$

现在我们看一下为什么 $P_k$ 和 $G_k$ 也应该是矩阵。 $P_k$ 是 $k$ 时刻估计过程的协方差，正如传感器的协方差矩阵 $R$ 一样， $P_k$ 也应该是一个矩阵，由于 $G_k$ 是每一步与这些矩阵相关的增益，因此 $G_k$ 也应该是一个矩阵，它应该包含这些矩阵中每个协方差的增益值。当然这些矩阵的尺寸应该取决于 $P_k$ 和 $G_k$ 所表示的内容。在我们当前的系统中， $P_k$ 是一个 1x1 的矩阵，即它是一个单一的值，因为它只包含了 $x_k$ 的协方差。增益值 $G_k$ 是一个 1x2 的矩阵，因为它涉及到状态 $x_k$ 的两个观测值 $z_k$ ，因为是两个传感器。

有了传感器融合的基本感念，我们再一次回到飞机飞行的例子中，我们的预测和更新方程如下，如前所示噪声的协方差还是定义在 0 到 200 之间。

预测：

$\hat x_k=A\hat x_{k-1}=1*\hat x_{k-1}=\hat x_{k-1}$
$P_{k}=AP_{k-1}A^T=1*P_{k-1}*1=P_{k-1}$

更新：

$G_k=P_kC^T(CP_kC^T+R)^{-1}=P_k \left[ \begin{matrix} 1 & 1 \end{matrix} \right] \left( \left[ \begin{matrix} 1 \\ 1 \end{matrix} \right] P_k \left[ \begin{matrix} 1 & 1 \end{matrix} \right] +\left[ \begin{matrix} 200 & 0 \\ 0 &180 \end{matrix} \right] \right) ^{-1}$

$\hat x_k\gets\hat x_k+G_k(z_k-C\hat x_k)=\hat x_k+G_k(z_k- \left[ \begin{matrix} 1 \\ 1 \end{matrix} \right] \hat x_k )$

$P_k=(I-G_kC)P_k= \left(1-G_k \left[ \begin{matrix} 1 \\ 1 \end{matrix} \right]\right) P_k$

回想一下在单变量系统中，我们的状态转换模型：

$x_k=ax_{k-1}+w_k$

$w_k$ 为k时刻的过程噪声，将a变换为矩阵：

$x_k=Ax_{k-1}+w_k$

事实上我们的预测更新模型中依然没有加入过程噪声，我们使用 $R$ 表示测量噪声 $v_k$ 的协方差，使用 $Q$ 表示过程噪声 $w_k$ 的协方差，那么 $P_k$ 可以写为：

$P_k=AP_{k-1}A^T+Q$

值得一提的是， $Q$ 矩阵即使很小的一个元素值，也有助于保持我们对状态的跟随。

下面所示是一个小型的传感器融合的例子，可适当的调整 $R$ 和 $Q$ ，也可以改变偏差，包括两个传感器的精度或噪声平均值，可以看到，当两个传感器在不同的方向上有偏差时，传感器融合能够提供一个更接近系统真实状态的一个近似。

image

15. 非线性处理

标准卡尔曼滤波前提条件之一就是高斯分布的 $x_k$ 预测后仍服从高斯分布，高斯分布的 $x_k$ 变换到观测空间后仍服从高斯分布，因此标准卡尔曼滤波只适合线性系统的处理。

但是现实世界中，很少有线性的。我们看到的树木、飞鸟、行云等都是弯曲的。由于传感器、电机等都是由物理材料制做的，因此他们同样呈现出一定的非线性。我们学过函数 $f(x)=ax^2+bx+c$ 、或者 $f(x)=sin(x)$ 、或者 $log(x)$ ，这些都是典型的非线性函数。

大家都知道，任何平滑的曲线都是可微的，可通过一系列连续的更小的线段来精确的表示这个曲线。
看一个例子 $f(x)=x^2$ 在 $\Delta x=10$ 的时候的细分：

image

那么，这种细分表示的方式又是如何帮助我们处理卡尔曼滤波中的非线性关系呢？假如我们有一个简单的滤波器，它的传感器符合以下线性方程：

$z_k=3x_k+2$

这个观测方程说明，传感器的读数总是等于状态值得3倍加上2，它是典型的线性关系。我们再看一个非线性传感器的方程：

$z_k=log_2(x_k)$

这就是一个典型的非线性关系，遇到这种情况我们可以计算非线性函数的一阶导数，即在每个给定点上函数的最佳线性近似。

知道了非线性函数的一阶导数，我们用非线性函数代替我们的观测方程中的 $Cx_k$ ，它的一阶导数为 $C_k$

模型：

$x_k=Ax_{k-1}+Bu_k$
$z_k=h(x_k)+v_k$

预测：

$\hat x_k=A\hat x_{k-1}+Bu_k$
$P_k=AP_{k-1}A^T$

更新：

$G_k=P_kC_k(C_kP_kC_k+R)^{-1}$
$\hat x_k\gets\hat x_k+G_k(z_k-h(\hat x_k))$
$P_k\gets(I-G_kC_k)P_k$

16. 扩展卡尔曼滤波

到这里我们已经对「EKF」有了初步认识，但是还需要了解下面一些内容：

如何在不知道其基本函数的情况下计算实际信号的一阶倒数
如何将单值非线性状态观测模型推广到多值系统

一阶倒数是这么定义的，当函数 $f(x)$ 的自变量 $x$ 在一点 $x_0$ 上产生一个增量 $\Delta x$ 时，函数输出值的增量与自变量增量的比值在趋于0时的极限如果存在，这个极限值即为函数 $f(x)$ 在 $x_0$ 处的导数，用公式表示：

$f'(x)=lim_{\Delta x\to 0}\frac{f(x+\Delta x)-f(x)}{\Delta x}$

在实际计算时，一阶导数可以这样近似代替：

$\frac{y_{k+1}-y_k}{timestep}$

在我们的实际系统中观测函数 $z_k$ 往往是状态函数 $x_k$ 的函数，此时就可以用连续差分的方法求一阶导数

$\frac{z_{k+1}-z_k}{x_{k+1}-x_k}$

现在看第二个问题，如何将单值非线性状态观测模型推广到多值系统，

状态观测方程：

$z_k=Cx_k$

对于一个有两个状态值和三个传感器的系统，我们可以将方程写为：

$z_k= \left[ \begin{matrix} c_{11} & c_{12} \\ c_{21} & c_{22} \\ c_{31} & c_{32} \end{matrix} \right] \left[ \begin{matrix} x_{k1} \\ x_{k2} \end{matrix} \right]= \left[ \begin{matrix} c_{11}x_{k1}+c_{12}x_{k2} \\ c_{21}x_{k1}+c_{22}x_{k2} \\ c_{31}x_{k1}+c_{32}x_{k2} \end{matrix} \right]$