扩展卡尔曼滤波的缺陷

存在正反馈

普通的扩展卡尔曼滤波（EKF），通过对动态方程的线性化，来估计状态与状态的协方差。比如有一个系统定义为

$\begin{aligned} \dot{x}_{t}&=f(x_t, u_t) + n_t \end{aligned}$

其中 $x_t$ 是系统状态， $u_t$ 是输入， $n_t$ 是系统噪声。假设某个时刻对状态的估计为 $\hat{x}_{t}$ ，并定义状态误差为 $e_{t}=x_{t}-\hat{x}_{t}$ ，则根据EKF，对 $f$ 在 $\hat{x}_{t}$ 处线性化（一阶泰勒展开），并注意到 $f(\hat{x}_{t_n}, u_t)=\dot{\hat{x}}_{t_{n+1}}$ ，有

$\begin{aligned} \dot{e}_{t}&=\dot{x}_{t}-\dot{\hat{x}}_{t} \\ &=f(x_t, u_t)-\dot{\hat{x}}_{t} + n_t\\ &\approx f(\hat{x}_{t}, u_t)+F_{\hat{x}_{t},u_t}(x_{t}-\hat{x}_{t}) - \dot{\hat{x}}_{t} + n_t\\ &=F_{\hat{x}_{t},u_t}(x_{t}-\hat{x}_{t}) + n_t\\ &=F_{\hat{x}_{t},u_t}e_t + n_t \end{aligned}$

上式是状态误差的线性传递方程，因此可以使用标准的卡尔曼滤波来估计协方差。

但是这里存在一个问题，上面误差传递方程中，系统矩阵 $F$ 依赖当前状态的估计量。在有噪声引入时，状态估计量是无法预测的，这就导致很难对上述系统方程做收敛性分析，实际上，任何EKF都没有收敛性保证。当状态估计值与真值差距较大时，直接导致依赖状态估计值的 $F$ 也有较大偏差，使用这样的 $F$ 继续传递误差后，又进一步放大误差，整个误差传递系统形成正反馈，最终导致滤波器发散。

实际上，在滤波器进行状态更新时也有类似问题。设系统的观测方程为

$y_t=h(x_t)+s_t$

其中 $s_t$ 是观测噪声。设实际观测与估计观测的误差为 $z_t=y_t-\hat{y}_t$ ，有

$\begin{aligned} z_t&=y_t-\hat{y}_t \\ &=h(x_t)-\hat{y}_t + s_t \\ &\approx h(\hat{x}_t)+H_{\hat{x}_t}(x_t-\hat{x}_t) - \hat{y}_t + s_t \\ &=H_{\hat{x}_t}e_t + s_t \end{aligned}$

上式即观测误差与状态误差之间的线性近似关系。同样的， $H_{\hat{x}_t}$ 与状态估计值有关，当真实状态与估计值差距较大时，利用上式进行更新可能起不到正面效果。

从上面的分析可知，普通EKF对初值的准确性要求挺高的，如果状态初值设定得与实际情况差距较大，很难让滤波器收敛。

非一致性

EKF还会导致另外一个问题，即所谓的不一致性。每当新的观测到来时，EKF会更新当前状态的估计，但是用于计算新状态的量，都是通过旧状态的线性化得来的。使用EKF的更新方法会出现不一致性，导致本来不该观测到信息的方向上观测到信息，在SLAM问题中这种现象比较明显，表现为较大的drift，以及过于乐观的协方差估计。因此，有很多针对该问题的方法，比如OC（Observability Constrain），FEJ（First Estimateed Jacobian）以及Robocentric等，这些方法都是对现有EKF框架的修补，而且使用起来都不容易。最后引用一张[1]中的图更直观的描述这个问题。

False Observability

不变扩展卡尔曼滤波

不变扩展卡尔曼滤波（简写为InEKF），可以较好的解决上面EKF存在的两个问题，而且有严格的数学推导作为保证。这里不介绍InEKF的收敛性和一致性的推导（实在有点复杂_!），主要关注其思想和用法。

InEKF的想法还是比较简单的——通过改变状态误差的定义方法，实现误差传递矩阵 $F$ 与状态估计值的独立。在EKF中，我们的状态误差直接定义为两个状态的差，即 $e_{t}=x_{t}-\hat{x}_{t}$ ，这太过于粗暴了，我们面对的是非线性系统，误差怎么都不应该是线性的形式。

现在我们以一个简单的问题为例，说明InEKF的用法。假设我们有一架无人机，上面装了IMU以及双目相机等传感器，我们打算对无人机的位姿进行估计。假设IMU的bias为零（之后会讨论有bias的情况），则IMU的系统方程为

$\begin{aligned} \dot{R}_t&=R_t[\tilde{w}_t-n_t^w]_\times \\ \dot{v}_t&=R_t(\tilde{a}_t-n_t^a)+g \\ \dot{p}_t&=v_t \end{aligned}$

其中 $\tilde{w}_t, \tilde{a}_t$ 是IMU的测量值， $n_t^w,n_t^a$ 分别是gyro和acc的噪声， $g$ 是重力设为已知，待估计的量就是 $R,v,p$ ，根据上一章李群的内容，我们发现这三个量刚好能构成一个 $SE_2(3)$ 群，于是我们将InEKF的状态量写为

$\chi_t= \begin{pmatrix} R_t & v_t & p_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}$

这里已经和EKF有很大不同了，一般EKF的状态量是个矢量，而InEKF的状态量是一个矩阵，而且构成群。既然是群，其误差也应该定义在群上，自然是不能再用减法了，设状态估计值为 $\hat{\chi}_t$ ，和真值的误差有两种形式

$\begin{aligned} \eta_t&=\chi_t^{-1}\hat{\chi}_t \\ \eta_t&=\hat{\chi}_t\chi_t^{-1} \end{aligned}$

第一种称为左不变误差（left-invariant），因为对 $\chi_t, \hat{\chi_t}$ 都左乘一个相同的群元素后，误差不变。同理，第二种为右不变误差（right-invariant）。具体使用哪种误差，要看该误差能否实现状态传递矩阵与状态估计值的独立。在很多SLAM问题中，右不变误差可以满足要求，因此下文都使用右不变误差。

误差传递

仿照EKF，现在推导这个误差的系统方程。

$\eta_t=\hat{\chi}_t\chi_t^{-1}= \begin{pmatrix} \hat{R}_tR_t^T & \hat{v}_t-\hat{R}_tR_t^Tv_t & \hat{p}_t-\hat{R}_tR_t^Tp_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}$

令

$\begin{aligned} \hat{R}_tR_t^T&=\eta_{R_t} \\ \hat{v}_t-\hat{R}_tR_t^Tv_t&=\xi_{v_t} \\ \hat{p}_t-\hat{R}_tR_t^Tp_t&=\xi_{p_t} \end{aligned}$

设 $\eta_{R_t}$ 对应的李代数为 $\xi_{R_t}$ ，即 $\eta_{R_t}=\text{Exp}(\xi_{R_t})$ ，由于 $\eta_{R_t}$ 是小量，用指数函数的一阶泰勒近似为 $\eta_{R_t}\approx I+[\xi_{R_t}]_\times$ ，则

$\dot{\eta_t}\approx \begin{pmatrix} [\dot{\xi}_{R_t}]_\times & \dot{\xi}_{v_t} & \dot{\xi}_{p_t} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix} = \frac{d}{dt}\Lambda \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}$

即 $\dot{\eta}_t$ 就是群 $SE_2(3)$ 在幺元处的切空间，其中

$\begin{aligned} \\ [\dot{\xi}_{R_t}]_\times\approx\dot{\eta}_{R_t}&=\dot{\hat{R}}_tR_t^T+\hat{R}_t\dot{R}_t^T \\ &=\hat{R}_t[\tilde{w}_t]_\times R_t^T-\hat{R}_t[w]_\times R_t^T \\ &=\hat{R}_t[\tilde{w}_t-w_t]_\times R_t^T \\ &=\hat{R}_t[n_t^w]_\times R_t^T \\ &=\hat{R}_t[n_t^w]_\times \hat{R}_t^T \hat{R}_tR_t^T \\ &=[\hat{R}_t n_t^w]_\times \eta_{R_t} \\ &\approx [\hat{R}_t n_t^w]_\times ( I+[\xi_{R_t}]_\times) \\ &\approx [\hat{R}_t n_t^w]_\times \end{aligned}$

上面最后一步忽略了高阶小量 $[\hat{R}_t n_t^w]_\times [\xi_{R_t}]_\times$ 。

同理有

$\begin{aligned} \dot{\xi}_{v_t} &= \dot{\hat{v}}_t - \dot{\eta}_{R_t}v_t-\eta_{R_t}\dot{v}_t \\ &=\hat{R}_t\tilde{a}_t+g-[\hat{R}_t n_t^w]_\times v_t-\hat{R}_tR_t^T(R_ta_t+g) \\ &=\hat{R}_t(\tilde{a}_t-a_t) + g-[\hat{R}_t n_t^w]_\times v_t-\hat{R}_tR_t^Tg \\ &\approx \hat{R}_tn_t^a+g-[\hat{R}_t n_t^w]_\times v_t-(I+[\xi_{R_t}]_\times)g \\ &=[g]_\times \xi_{R_t}+([v_t]_\times \hat{R}_t) n_t^w+\hat{R}_tn_t^a \end{aligned}$

以及

$\begin{aligned} \dot{\xi}_{p_t} &= \dot{\hat{p}}_t - \dot{\eta}_{R_t}p_t-\eta_{R_t}\dot{p}_t \\ &=\hat{v}_t-[\hat{R}_t n_t^w]_\times p_t - \hat{R}_tR_t^Tv_t \\ &=(\hat{v}_t-\hat{R}_tR_t^Tv_t)-[\hat{R}_t n_t^w]_\times p_t \\ &=\xi_{v_t}+([p_t]_\times \hat{R}_t) n_t^w \end{aligned}$

综上有

$\begin{aligned} \frac{d}{dt}\Lambda \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}&= \begin{pmatrix} [\hat{R}_t n_t^w]_\times & [g]_\times \xi_{R_t}+([v_t]_\times \hat{R}_t) n_t^w+\hat{R}_tn_t^a & \xi_{v_t}+([p_t]_\times \hat{R}_t) n_t^w \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix} \\ &=\Lambda\Bigg[ \begin{pmatrix} 0 & 0 & 0 \\ [g]_\times & 0 & 0 \\ 0 & I & 0 \end{pmatrix} \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}+ \begin{pmatrix} \hat{R}_tn_t^w \\ ([v_t]_\times \hat{R}_t) n_t^w+\hat{R}_tn_t^a \\ ([p_t]_\times \hat{R}_t) n_t^w \end{pmatrix} \Bigg] \ \ \ \ \text{分离状态项和噪声项} \\ &=\Lambda\Bigg[ \begin{pmatrix} 0 & 0 & 0 \\ [g]_\times & 0 & 0 \\ 0 & I & 0 \end{pmatrix} \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix}+ \begin{pmatrix} \hat{R}_t & 0 & 0 \\ [\hat{v}_t]_\times\hat{R}_t & \hat{R}_t & 0 \\ [\hat{p}_t]_\times\hat{R}_t & 0 & \hat{R}_t \end{pmatrix} \begin{pmatrix} n_t^w \\ n_t^a \\ 0 \end{pmatrix} \Bigg] \end{aligned}$

令 $\xi_t=(\xi_{R_t}^T, \xi_{v_t}^T, \xi_{p_t}^T)^T$ ，以及 $n_t=(n_t^w, n_t^a, 0)$ ，可得误差的线性系统方程

$\dot{\xi}_t=F_t\xi_t+Ad_{\hat{\chi}_t}n_t$

其中

$\begin{aligned} F_t&= \begin{pmatrix} 0 & 0 & 0 \\ [g]_\times & 0 & 0 \\ 0 & I & 0 \end{pmatrix} \\ Ad_{\hat{\chi}_t}&= \begin{pmatrix} \hat{R}_t & 0 & 0 \\ [\hat{v}_t]_\times\hat{R}_t & \hat{R}_t & 0 \\ [\hat{p}_t]_\times\hat{R}_t & 0 & \hat{R}_t \end{pmatrix} \end{aligned}$

与普通EKF的误差系统方程相比，最大不同就是系统矩阵 $F_t$ 是一个常量！和输入以及状态估计值均无关！而噪声项前面的矩阵，刚好是当前状态矩阵的伴随。基于这样的系统方程，可以证明其收敛性还有一致性都有较好的保证。

后面的事情就和普通EKF一样了，根据系统方程估计下一时刻的状态（使用RK4积分等方法），然后对误差系统方程积分，得到状态转移矩阵，并传递协方差矩阵，即

$P_t=\Phi_tP_{t-1}\Phi_t^T + Ad_{\hat{\chi}_t}Q_tAd_{\hat{\chi}_t}^T$

其中 $\Phi_t$ 是通过 $F_t$ 积分后得到的状态转移矩阵， $Q_t=E(n_tn_t^T)$ 是系统噪声的协方差矩阵。由于系统矩阵是常量，积分可以变得非常简单，甚至直接写出解析式。这也算是InEKF的另一个优点吧。

状态更新

假设地图中有很多landmark，用 $m_1, m_2, ..., m_k$ 表示，无人机上的传感器可以观测到这些landmark相对于自身的位置，分别用 $y_t^1, y_t^2, ..., y_t^k$ 表示，设每个观测的噪声为 $s_t^1, s_t^2, ..., s_t^k$ ，则观测方程就是

$\begin{aligned} y_t^1 &= R_t^T(m_1-p_t) + s_t^1\\ y_t^2 &= R_t^T(m_2-p_t) + s_t^2\\ &\vdots \\ y_t^k &= R_t^T(m_k-p_t) + s_t^k \end{aligned}$

拿其中一个观测来举例，我们可以发现观测方程可以写为如下形式

$\begin{aligned} \begin{pmatrix} y_i \\ 0 \\ 1 \end{pmatrix}&= \begin{pmatrix} R_t^T & -R_t^Tv_t & -R_t^Tp_t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} m_i \\ 0 \\ 1 \end{pmatrix}+ \begin{pmatrix} s_t^i \\ 0 \\ 0 \end{pmatrix} \end{aligned}$

注意等号右边第一项的矩阵是状态矩阵的逆，令 $Y_t^i=(y_t^i, 0, 1), M^i=(m_i, 0, 1), S_t^i=(s_t^i, 0, 0)$ （由于不方便书写，所以都写为行向量的形式，实际上它们都是列向量），有

$Y_t^i=\chi_t^{-1}M^i+S_t^i$

现在我们定义观测误差。如果定义为

$Z_t^i=Y_t^i-\hat{\chi}_t^{-1}M^i$

那么就与普通的EKF无异。考虑到我们的状态量是一个群元素，可以定义观测误差为（这样定义的目的在后面的推导中会变清晰）

$Z_t^i=\hat{\chi}_tY_t^i-M^i$

这样就可以得到

$\begin{aligned} Z_t^i&=\hat{\chi}_t (\chi_t^{-1}M^i+S_t^i) - M^i \\ &=\hat{\chi}_t\chi_t^{-1}M^i - M^i + \hat{\chi}_tS_t^i \\ &=\eta_t M^i - M^i + \hat{\chi}_tS_t^i \end{aligned}$

将所有的观测误差按照列向量的方式堆叠起来，有

$z_t= \begin{pmatrix} \eta_t M^1 - M^1 + \hat{\chi}_tS_t^1 \\ \vdots \\ \eta_t M^k - M^k + \hat{\chi}_tS_t^k \end{pmatrix}$

和EKF一样，现在需要求出观测误差与状态误差之间的线性关系。利用状态误差的一阶近似 $\eta_t\approx I+\Lambda(\xi_t)$ ，有

$\begin{aligned} z_t &\approx \begin{pmatrix} (I+\Lambda(\xi_t)) M^1 - M^1 + \hat{\chi}_tS_t^1 \\ \vdots \\ (I+\Lambda(\xi_t)) M^k - M^k + \hat{\chi}_tS_t^k \end{pmatrix} \\ &= \begin{pmatrix} \Lambda(\xi_t)M^1 + \hat{\chi}_tS_t^1 \\ \vdots \\ \Lambda(\xi_t)M^k + \hat{\chi}_tS_t^k \end{pmatrix} \\ \end{aligned}$

将 $M^k, S_t^k, \hat{\chi}_t,\xi_t$ 的具体表达式带入上式计算后，每三行就有两行是全零，为了使计算和书写更紧凑，可以将 $z_t$ 中全是零的行去掉，得到 $\tilde{z}_t$ ，即

$\begin{aligned} \tilde{z}_t&= \begin{pmatrix} [\xi_{R_t}]_\times m_1 + \xi_{p_t} + \hat{R}_t s_t^1 \\ \vdots \\ [\xi_{R_t}]_\times m_k + \xi_{p_t} + \hat{R}_t s_t^k \end{pmatrix}\\ &= \begin{pmatrix} -[m_1]_\times & 0_{3\times3} & I_{3\times3} \\ \vdots \\ -[m_k]_\times & 0_{3\times3} & I_{3\times3} \end{pmatrix} \begin{pmatrix} \xi_{R_t} \\ \xi_{v_t} \\ \xi_{p_t} \end{pmatrix} + \begin{pmatrix} \hat{R}_t s_t^1 \\ \vdots \\ \hat{R}_t s_t^k \end{pmatrix} \\ &=H_t\xi_t+s_t \end{aligned}$

下面考虑怎么根据观测误差更新InEKF的状态。回想一下普通EKF的更新方式

$\hat{x}_t^+=\hat{x}_t+K_t[y_t-h(\hat{x}_t)]=\hat{x}_t+K_tz_t$

等价于状态误差的更新（上式两边减去 $x_t$ ）

$e_t^+=e_t+K_tz_t$

其中 $K_t$ 是卡尔曼增益。

同理，根据InEKF的误差定义，更新后的误差

$\begin{aligned} \eta_t^+&=\hat{\chi}_t^+\chi_t^{-1} \\ &=\hat{\chi}_t^+\hat{\chi}_t^{-1}\hat{\chi}_t\chi_t^{-1} \\ &=\hat{\chi}_t^+\hat{\chi}_t^{-1}\eta_t \end{aligned}$

其中 $\hat{\chi}_t^+\hat{\chi}_t^{-1}$ 就是更新前后状态的差异，可以设

$\hat{\chi}_t^+\hat{\chi}_t^{-1}=\text{Exp}(K_tz_t)$

就得InEKF的状态误差的更新方程

$\eta_t^+=\text{Exp}(K_tz_t)\eta_t$

以及状态的更新方程

$\hat{\chi}_t^+=\text{Exp}(K_tz_t)\hat{\chi}_t$

即InEKF采用的是“指数更新”。最后来看看增益 $K_t$ 怎么算，由误差的更新方程以及观测误差的定义可得

$\begin{aligned} \eta_t^+ &= \text{Exp}(K_tz_t)\eta_t \end{aligned}$

利用指数函数的一阶近似 $\text{Exp}(\xi)\approx I+\Lambda(\xi)$ ，有

$I+\Lambda(\xi_t^+)=[I + \Lambda(K_tz_t)][I+\Lambda(\xi_t)]$

忽略高阶小量 $\Lambda()\cdot\Lambda()$ ，有

$\begin{aligned} \xi_t^+ &= \xi_t+K_t z_t \end{aligned}$

上式与普通EKF中的误差更新方程 $e_t^+=e_t+K_tz_t$ 有相同的形式，所以增益 $K_t$ 的计算和普通的EKF完全相同。上式也可以用更紧凑的 $\tilde{z}_t$ 代替，对应的更紧凑的增益记为 $\tilde{K}_t$ ，则

$\tilde{K}_t = P_tH_t^T(H_tP_tH_t^T+V_t)^{-1}$

其中， $P_t$ 是状态误差 $\xi_t$ 的协方差矩阵， $H_t$ 是之前推出的 $\tilde{z}_t$ 与 $\xi_t$ 之间的观测矩阵， $V_t=E(s_ts_t^T)$ 是观测误差的协方差矩阵（注意， $s_t$ 中的每一项噪声都被 $\hat{R}_t$ 旋转过）。状态的更新自然就是

$\begin{aligned} \hat{\chi}_t^+&=\text{Exp}(\tilde{K}_t\tilde{z}_t)\hat{\chi}_t \\ P_t^+&=(I-\tilde{K}_tH_t)P_t \end{aligned}$

不变扩展卡尔曼滤波（二）：原理与推导

Invariant Extented Kalman Filter - The Idea

扩展卡尔曼滤波的缺陷

存在正反馈

非一致性

不变扩展卡尔曼滤波

误差传递

状态更新

References

CATALOG

FEATURED TAGS

FRIENDS