不变扩展卡尔曼滤波(三):一个更通用的框架

Invariant Extented Kalman Filter - A More General Framework

Posted by Jerry Zhao on April 28, 2019

复杂系统下的不变扩展卡尔曼滤波

由前面的文章可知,不变性要靠合理的状态误差设计来实现。但在更复杂的系统中,为所有状态量找到一个满足不变性的状态误差是很困难的,甚至一些状态是不可能被表示为群运算的。在存在这类状态量的系统中,有没有可能保留InEKF的主要优点呢(主要指一致性方面的优点)?答案是肯定的。

考虑这样一类系统,其系统方程如下:

抛开矢量不看,上式就是之前讨论的普通系统方程,引入矢量后,系统的变化不仅依赖当前时刻的状态和输入,还依赖另外一个随着时间变化的量。在普通的EKF中,可以将也融入到需要估计的状态量中。这里之所以分开写,是为了在InEKF中区分可以被描述为群元素的状态量,和不能被描述为群元素的量

以一个完整的IMU系统为例,系统方程为:

其中分别为IMU的gyro bias和accelerator bias,他们满足Random Walk模型,即随时间的变化率是一个满足高斯分布的白噪声。

由之前的文章可知,能够构成一个群元素并且使得最终导出的误差系统方程满足不变性(系统矩阵与状态无关),但如果将两个bias也放入群中,就很难找到一个状态误差使其满足不变性了。也就是说,就是上文中的,而就是上文中的

为了处理这类系统,可以将状态误差定义为如下形式:

即右不变误差与普通线性误差的笛卡尔积,是一种混合误差。

混合误差的系统方程

现在需要推导在混合误差下,该误差的系统方程是怎样的。直接以上文的IMU系统为例,令

以及

推导方式模仿第二篇文章中普通系统的推导,也就是计算:

其中的定义和上一篇文章中一样

仅在的计算中有少许不同,分别为

以及

以及

由于是普通的线性误差,推导就比较简单了,即

综上,模仿上一篇文章的处理方式,可以得到整个系统状态误差的系统方程:

将其写为更紧凑的形式,容易看出带有bias的IMU系统与之前不带bias的IMU系统之间的联系

其中就是上一篇文章中,不带bias的IMU系统的系统矩阵,是其噪声矩阵。带有bias的IMU系统,只是在原来的系统矩阵上进行了增广。这个结论适用于所有满足本文开头所定义的系统形式。

可以看到,在这种混合误差形式下,系统矩阵不再是一个常量了,每个时刻的系统矩阵与当前状态的估计值有关。但是,这种联系只通过bias或噪声等小量引入。可以证明对于这样的系统方程,一致性仍然能够保证。同时大量的实验也表明,这种“非完美”的InEKF在鲁棒性和收敛性上仍然优于传统的EKF。

混合状态的更新

之前已经知道,观测误差与状态误差的线性关系为

其中是观测噪声。

现在设系统的观测量不变(即是无法直接观测到的),但是状态误差变成了,显然线性关系应该变为

以带有bias的IMU系统为例,就是上一篇文章中的观测矩阵,即

其中是观测到的landmark在世界系下的坐标。而观测误差与bias的误差没有直接关系,即

同理,线性状态误差的更新变为了:

卡尔曼增益的计算都一样,即

只是现在的,状态的协方差矩阵在维数上也有相应改变。

最终,混合状态的更新方式也采用混合的形式,能表示为群元素的部分采用指数更新,不能表示为群元素的部分采用普通的线性更新,即

总结

设系统方程满足如下形式:

其中都是需要估计的量,但是是一个群元素(比如中的元素),是一个矢量,一般不可直接观测。则该系统在InEKF框架下,具有如下形式的系统矩阵和观测矩阵

且卡尔曼增益

状态的更新方式为

References

  1. Non-linear state error based extended Kalman flters with applications to navigation
  2. Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation