滤波笔记四：扩展卡尔曼滤波

情境：

有一个正在移动的路人，他的状态通过二维位置和二维速度表示：$x=(p_x,p_y,v_x,v_y{)}^T$，即标准等速模型。我们有两种特定传感器：1. 激光雷达 2. 雷达。

• 激光雷达的测量空间：$z_1=(p_x,p_y)$。
• 雷达的测量空间： $z_2=(\rho ,\phi ,\dot{\rho }{)}^T$;

1. Introduction

假设我们使用高斯分布描述了预测状态 $x$，如果我们把这个高斯分布映射到非线性函数 $h$， 那么结果就不再是一个高斯分布了，这时卡尔曼滤波也不再适用：

在以上情境中，当我们有一个正态分布，其直方图如下图可见，对于每个值，计算其对应的 $h$，如预测的位置和速度映射到极坐标上的距离(range)，方向(bearing)和距离变化率(range rate):
$$h\left(x^{\prime }\right)=\left(\begin{array}{c}\rho \\ \varphi \\ \dot{\rho }\end{array}\right)=\left(\begin{array}{c}\sqrt{p_x^{\prime 2}+p_y^{\prime 2}}\\ \arctan \left(p_y^{\prime }/p_x^{\prime }\right)\\ \frac{p_x^{\prime }{v}_x^{\prime }+p_y^{\prime }{v}_y^{\prime }}{\sqrt{p_x^{\prime 2}+p_y^{\prime 2}}}\end{array}\right)$$

在图像中使用正反切可见，新的对应直方图并不再是一个高斯分布，因为 $h$ 是非线性的，因此这里不能使用卡尔曼方程。

一种可行的方案是，对 $h(x)$ 函数进行线性化操作。这也是扩展卡尔曼滤波器的核心思想。因此在这个例子中，我们必须使用在原始高斯分布的均值位置和 $h$ 相切的一个线性函数，近似出测量值函数，这样得到的结果满足高斯分布。

那么该如何对非线性函数进行线性化处理呢？

2. Taylor Series Expansion

2.1 One-dimensional Taylor Series Expansion

扩展卡尔曼滤波器使用了泰勒展开法，我们首先评估在均值位置 $\mu$ 的非线性函数，对式子求一阶泰勒展开得:
$$h(x)\approx h(\mu )+\frac{\partial h(\mu )}{\partial x}(x-\mu )$$

从式中可以看出，

1. 首先评估在均值位置 $\mu$ 的非线性函数 $h(\mu )$ ;
2. 使用围绕 $\mu$ 的斜度来推断这条线的斜度，由 $h$ 的第一个导数给出： $\frac{\partial h(\mu )}{\partial x}$

这样可得上图红色直线所示的线性函数。

2.2 Multivariate Taylor Series

多维泰勒系数展开：
$$T(x)=f(a)+\frac{f^{\prime }(a)}{1!}(x-a)+\frac{f^{\prime \prime }(a)}{2!}(x-a{)}^2+\frac{f^{\prime \prime \prime }(a)}{3!}(x-a{)}^3+\dots$$

已知之前所述函数 $h$ 将预测状态 $x^{\prime }=(p_x^{\prime },p_y^{\prime },v_x^{\prime },v_y^{\prime }{)}^T$ 映射到测量空间 $z=(\rho ,\phi ,\dot{\rho }{)}^T$，这里是一个多维方程，需要使用多维泰勒展开来线性近似函数 $h$，通常多维泰勒系数展开公式写为：
$$T(x)=f(a)+(x-a{)}^{T}Df(a)+\frac{1}{2!}(x-a{)}^T{D}^{2}f(a)(x-a)+\dots$$

其中 $Df(a)$ 为称为 Jacobian matrix；$D^{2}f(a)$ 被称为 Hessian matrix。他们分别表示了多维方程的一阶和二阶导。

为了获得函数 $h$ 的线性近似，我们最多只保留展开项至雅克比矩阵 $Df(a)$；忽略海塞矩阵 $D^{2}f(a)$ 及更高阶项。假设 $(x-a)$ 值很小，$(x-a{)}^2$ 的值就更小了------- EKF 假设比 Jacobian 更高阶项可忽略。

3. Jacobian

已知前式的一阶泰勒展开：
$$h(x)\approx h(\mu )+\frac{\partial h(\mu )}{\partial x}(x-\mu )$$
我们把这个例子归纳到一个更高的维度，那么 $h(\mu )$ 对应于 $x$ 的导数，被称作雅克比矩阵：它是一个矩阵，包含所有偏导数。

3.1 Jacboian Matrix

常规雅克比矩阵公式为：
$$H_j=\left[\begin{array}{cccc}\frac{\partial h_1}{\partial x_1}& \frac{\partial h_1}{\partial x_2}& \dots & \frac{\partial h_1}{\partial x_n}\\ \frac{\partial n_2}{\partial x_1}& \frac{\partial h_2}{\partial x_2}& \cdots & \frac{\partial h_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial h_m}{\partial x_1}& \frac{\partial h_m}{\partial x_2}& \cdots & \frac{\partial h_m}{\partial x_n}\end{array}\right]$$

对于示例中已知的测量空间：$z=(\rho ,\phi ,\dot{\rho }{)}^T$；状态是一个向量，其包括四个分量：$x=(p_x,p_y,v_x,v_y{)}^T$ ------ 因此，雅可比矩阵 $H_j$ 在这里是一个三行四列的矩阵：

$$H_j=\left[\begin{array}{llll}\frac{\partial \rho }{\partial p_x}& \frac{\partial \rho }{\partial p_y}& \frac{\partial \rho }{\partial v_x}& \frac{\partial \rho }{\partial v_y}\\ \frac{\partial \phi }{\partial p_x}& \frac{\partial \phi }{\partial p_y}& \frac{\partial \phi }{\partial v_x}& \frac{\partial \phi }{\partial v_y}\\ \frac{\partial \dot{\rho }}{\partial p_x}& \frac{\partial \dot{\rho }}{\partial p_y}& \frac{\partial \dot{\rho }}{\partial v_x}& \frac{\partial \dot{\rho }}{\partial v_y}\end{array}\right]$$

计算过所有偏导数后，得到矩阵¹：
$$H_j=\left[\begin{array}{cccc}\frac{p_x}{\sqrt{p_x^2+p_y^2}}& \frac{p_y}{\sqrt{p_x^2+p_y^2}}& 0& 0\\ -\frac{p_y}{p_x^2+p_y^2}& \frac{p_x}{p_x^2+p_y^2}& 0& 0\\ \frac{p_y\left(v_x{p}_y-v_y{p}_x\right)}{{\left(p_x^2+p_y^2\right)}^{3/2}}& \frac{p_x\left(v_y{p}_x-v_x{p}_y\right)}{{\left(p_x^2+p_y^2\right)}^{3/2}}& \frac{p_x}{\sqrt{p_x^2+p_y^2}}& \frac{p_y}{\sqrt{p_x^2+p_y^2}}\end{array}\right]$$

4. Differences between KF and EKF

主要区别有以下几点：

• 在计算 $P^{\prime }$ 时，矩阵 $F$ 被 $F_j$ 替换；
• 在计算 $S,K,P$ 时，KF 中的矩阵 $H$ 被雅克比矩阵 $H_j$ 所替换；
• 在计算 $x^{\prime }$ 时，矩阵 $F$ 被预测更新函数 $f$ 所替换；
• 在计算 $y$ 时，矩阵 $H$ 由函数 $h$ 所替换。

（不一定全部要从 KF 变为 EKF，主要看有哪些位置使用的是非线性模型）
注意，线性逼近只针对了 $F_j$, $H_j$，而在预测和更新误差的时候都是使用了真实的 $f$, $h$，所以这个算法的误差主要出现在矩阵运算中。
如在上面的例子中，由于预测阶段还是线性的，所以 $F$ 并不需要被 $F_j$ 所替换；$F$ 也不需要被函数 $f$ 所替换。在下图右侧中，除了这两个公式外，都是使用的线性逼近。

可能上图对于线性逼近的共视还不算特别清晰，这里再贴一个 Wiki 的全部流程：

5. Overall Process

点击跳转：
滤波笔记一：卡尔曼滤波.
滤波笔记二：无迹卡尔曼滤波 CTRV&&CTRA模型.
滤波笔记三：粒子滤波.

---

1. 偏导计算过程：
   $$\begin{array}{l}\frac{\partial \rho }{\partial p_x}=\frac{\partial }{\partial p_x}\left(\sqrt{p_x^2+p_y^2}\right)=\frac{2p_x}{\sqrt{p_x^2+p_y^2}}=\frac{p_x}{\sqrt{p_x^2+p_y^2}}\\ \frac{\partial \rho }{\partial p_y}=\frac{\partial }{\partial p_y}\left(\sqrt{p_x^2+p_y^2}\right)=\frac{2p_y}{\sqrt[2]{p_x^2+p_y^2}}=\frac{p_y}{\sqrt{p_x^2+p_y^2}}\\ \frac{\partial \rho }{\partial v_x}=\frac{\partial }{\partial v_x}\left(\sqrt{p_x^2+p_y^2}\right)=0\\ \frac{\partial \rho }{\partial v_y}=\frac{\partial }{\partial v_y}\left(\sqrt{p_x^2+p_y^2}\right)=0\\ \frac{\partial \phi }{\partial p_x}=\frac{\partial }{\partial p_x}\arctan \left(p_y/p_x\right)=\frac{1}{{\left(\frac{p_y}{p_x}\right)}^2+1}\left(-\frac{p_y}{p_x^2}\right)=-\frac{p_y}{p_x^2+p_y^2}\\ \frac{\partial \phi }{\partial p_y}=\frac{\partial }{\partial p_y}\arctan \left(p_y/p_x\right)=\frac{1}{{\left(\frac{p_y}{p_x}\right)}^2+1}\left(\frac{1}{p_x}\right)=\frac{p_x^2}{p_x^2+p_y^2}\frac{1}{p_x}=\frac{p_x}{p_x^2+p_y^2}\\ \frac{\partial \phi }{\partial v_x}=\frac{\partial }{\partial v_x}\arctan \left(p_y/p_x\right)=0\\ \frac{\partial \phi }{\partial v_y}=\frac{\partial }{\partial v_y}\arctan \left(p_y/p_x\right)=0\\ \frac{\partial \dot{\rho }}{\partial p_x}=\frac{\partial }{\partial p_x}\left(\frac{p_x{v}_x+p_y{v}_y}{\sqrt{p_x^2+p_y^2}}\right)\end{array}$$ ↩︎