信息滤波器(Information Filter)的推导过程
信息滤波(Information Filter)的推导过程
本文是Jesse Chen在阅读Information Filter时候所做的笔记，原创。如有转载，请注明出处。谢谢。
文章目录
- 信息滤波(Information Filter)的推导过程
- 介绍
- 正则参数/Canonical Parameterization
- 信息滤波的推导过程
介绍
Kalman Filter的对偶形式是信息滤波器(information filter或者IF)。和EKF一样，信息滤波器也是用高斯分布表示置信度(belief)。IF和Kalman Filter的主要区别是表达高斯分布的方式有区别。
在Kalman Filter族的算法中，高斯函数采用它们的矩来表示（均值和协方差）。而Information Filter采用正则参数（信息矩阵和信息向量）表示高斯函数。这两种表示形式互为对偶。
正则参数/Canonical Parameterization
多维高斯的正则参数表示为一个矩阵 Ω \Omega Ω和一个向量 ξ \boldsymbol{\xi} ξ。信息矩阵 Ω \Omega Ω是协方差矩阵的逆：
Ω = Σ − 1 \Omega = \Sigma^{-1} Ω=Σ−1
信息向量 ξ \boldsymbol{\xi} ξ用矩表示为：
ξ = Σ − 1 μ \boldsymbol{\xi} = \Sigma^{-1}\boldsymbol{\mu} ξ=Σ−1μ
易知 Ω \Omega Ω和 ξ \boldsymbol{\xi} ξ是高斯概率分布的完备参数。由正则参数推导矩参数的方程如下：
Σ = Ω − 1 μ = Ω − 1 ξ \begin{aligned} \Sigma &= \Omega^{-1} \\ \mu &= \Omega^{-1}\boldsymbol{\xi} \end{aligned} Σμ=Ω−1=Ω−1ξ
多维高斯概率分布函数：
P ( x ) = det ( 2 π Σ ) − 1 2 exp { − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) } P\left(\mathbf{x}\right) = \det\left(2\pi\Sigma\right)^{-\frac{1}{2}}\exp\left\{-\frac{1}{2}\left(\mathbf{x} - \boldsymbol{\mu}\right)^T\Sigma^{-1}\left(\mathbf{x} - \boldsymbol{\mu}\right)\right\} P(x)=det(2πΣ)−21exp{−21(x−μ)TΣ−1(x−μ)}
将 − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) -\frac{1}{2}\left(\mathbf{x} - \boldsymbol{\mu}\right)^T\Sigma^{-1}\left(\mathbf{x} - \boldsymbol{\mu}\right) −21(x−μ)TΣ−1(x−μ)展开，可以得到 P ( x ) P\left(\mathbf{x}\right) P(x)的表达式:
P ( x ) = det ( 2 π Σ ) − 1 2 exp { − 1 2 x T Σ − 1 x + x T Σ − 1 μ − 1 2 μ T Σ − 1 μ } = det ( 2 π Σ ) − 1 2 exp { − 1 2 μ T Σ − 1 μ } ⎵ const. exp { − 1 2 x T Σ − 1 x + x T Σ − 1 μ } \begin{aligned} P\left(\mathbf{x}\right) &= \det\left(2\pi\Sigma\right)^{-\frac{1}{2}}\exp\left\{-\frac{1}{2}\mathbf{x}^T\Sigma^{-1}\mathbf{x} + \mathbf{x}^T\Sigma^{-1}\boldsymbol{\mu} - \frac{1}{2}\boldsymbol{\mu}^T\Sigma^{-1}\boldsymbol{\mu}\right\} \\ &= \underbrace{\det\left(2\pi\Sigma\right)^{-\frac{1}{2}}\exp\left\{-\frac{1}{2}\boldsymbol{\mu}^T\Sigma^{-1}\boldsymbol{\mu}\right\}}_{\text{const.}}\exp\left\{-\frac{1}{2}\mathbf{x}^T\Sigma^{-1}\mathbf{x} + \mathbf{x}^T\Sigma^{-1}\boldsymbol{\mu}\right\} \end{aligned} P(x)=det(2πΣ)−21exp{−21xTΣ−1x+xTΣ−1μ−21μTΣ−1μ}=const. det(2πΣ)−21exp{−21μTΣ−1μ}exp{−21xTΣ−1x+xTΣ−1μ}
标为const的部分和 x \mathbf{x} x无关，因此我们可以用 η \eta η代替这部分。
P ( x ) = η exp { − 1 2 x T Ω x + x T ξ } P\left(\mathbf{x}\right) = \eta\exp\left\{-\frac{1}{2}\mathbf{x}^{T}\Omega\mathbf{x} + \mathbf{x}^T\boldsymbol{\xi}\right\} P(x)=ηexp{−21xTΩx+xTξ}
信息滤波的推导过程
和Kalman滤波器一样，系统的状态转移方程和测量方程如下：
x t = A t x t − 1 + B t u t + ε t z t = C t x t + δ t \begin{aligned} \mathbf{x}_t &= A_t\mathbf{x}_{t-1} + B_t\mathbf{u}_t + \boldsymbol{\varepsilon}_t \\ \mathbf{z}_t &= C_t\mathbf{x}_t + \boldsymbol{\delta}_t \end{aligned} xtzt=Atxt−1+Btut+εt=Ctxt+δt
因为信息滤波器是Kalman滤波器的对偶，我们可以从Kalman方程对应到信息滤波器方程。Kalman滤波器方程：
Predict
μ ˉ t = A t μ t − 1 + B t u t Σ ˉ t = A t Σ t − 1 A t T + R t \begin{aligned} \bar{\boldsymbol{\mu}}_t &= A_t\boldsymbol{\mu}_{t-1} + B_t\mathbf{u}_t \\ \bar{\Sigma}_t &= A_t\Sigma_{t-1}A_t^T + R_t \end{aligned} μˉtΣˉt=Atμt−1+Btut=AtΣt−1AtT+Rt
Update
K t = Σ ˉ t C t T ( C t Σ ˉ t C t T + Q t ) − 1 μ t = μ ˉ t + K t ( z t − C t μ ˉ t ) Σ t = ( I − K t C t ) Σ ˉ t \begin{aligned} K_t &= \bar{\Sigma}_t C_t^T\left(C_t\bar{\Sigma}_tC_t^T + Q_t\right)^{-1} \\ \boldsymbol{\mu}_t &= \bar{\boldsymbol{\mu}}_t + K_t\left(\mathbf{z}_t - C_t\bar{\mu}_t\right) \\ \Sigma_t &= \left(I - K_tC_t\right)\bar{\Sigma}_t \end{aligned} KtμtΣt=ΣˉtCtT(CtΣˉtCtT+Qt)−1=μˉt+Kt(zt−Ctμˉt)=(I−KtCt)Σˉt
推导过程如下：根据正则参数和矩参数的关系，
Ω = Σ − 1 ξ = Σ − 1 μ \begin{aligned} \Omega &= \Sigma^{-1} \\ \boldsymbol{\xi} &= \Sigma^{-1}\boldsymbol{\mu} \end{aligned} Ωξ=Σ−1=Σ−1μ
对于信息滤波器，预测信息矩阵的方法如下：
Ω ˉ − 1 = A t Ω t − 1 − 1 A t T + R t ⇒ Ω ˉ = ( A t Ω t − 1 − 1 A t T + R t ) − 1 \begin{aligned} \bar{\Omega}^{-1} &= A_t\Omega_{t-1}^{-1}A_{t}^T + R_t \\ \Rightarrow\qquad\bar{\Omega} &= \left(A_t\Omega_{t-1}^{-1}A_t^T + R_t\right)^{-1} \end{aligned} Ωˉ−1⇒Ωˉ=AtΩt−1−1AtT+Rt=(AtΩt−1−1AtT+Rt)−1
预测信息向量的方法如下：
μ ˉ t = Ω ˉ t − 1 ξ ˉ t μ t = Ω t − 1 − 1 ξ t − 1 μ ˉ t = A t μ t − 1 + B t u t ⇒ Ω ˉ t − 1 ξ ˉ t = A t Ω t − 1 − 1 ξ t − 1 + B t u t ⇒ ξ ˉ t = Ω ˉ t ( A t Ω t − 1 − 1 ξ t − 1 + B t u t ) \begin{aligned} \bar{\boldsymbol{\mu}}_t &= \bar{\Omega}_t^{-1}\bar{\boldsymbol{\xi}}_t \\ \boldsymbol{\mu}_t &= \Omega_{t-1}^{-1}\boldsymbol{\xi}_{t-1} \\ \bar{\boldsymbol{\mu}}_t &= A_t\boldsymbol{\mu}_{t-1} + B_t\mathbf{u}_t \\ \Rightarrow\qquad\bar{\Omega}_t^{-1}\bar{\boldsymbol{\xi}}_t &= A_t\Omega_{t-1}^{-1}\boldsymbol{\xi}_{t-1} + B_t\mathbf{u}_t \\ \Rightarrow\qquad\bar{\boldsymbol{\xi}}_t &= \bar{\Omega}_t\left(A_t\Omega_{t-1}^{-1}\boldsymbol{\xi}_{t-1} + B_t\mathbf{u}_t\right) \end{aligned} μˉtμtμˉt⇒Ωˉt−1ξˉt⇒ξˉt=Ωˉt−1ξˉt=Ωt−1−1ξt−1=Atμt−1+Btut=AtΩt−1−1ξt−1+Btut=Ωˉt(AtΩt−1−1ξt−1+Btut)
下面来计算如何用正则参数表达 K t K_t Kt。在计算用正则参数表达 K t K_t Kt时，需要用到Matrix Inverse Lemma
( A + B C D ) − 1 = A − 1 − A − 1 B ( C − 1 + D A − 1 B ) − 1 D A − 1 \left(A + BCD\right)^{-1} = A^{-1} - A^{-1}B\left(C^{-1} + DA^{-1}B\right)^{-1}DA^{-1} (A+BCD)−1=A−1−A−1B(C−1+DA−1B)−1DA−1
对于 K t = Σ ˉ t C t T ( C t Σ ˉ t C t T + Q t ) − 1 K_t = \bar{\Sigma}_t C_t^T\left(C_t\bar{\Sigma}_t C_t^T + Q_t\right)^{-1} Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1中的 ( C t Σ ˉ t C t T + Q t ) − 1 \left(C_t\bar{\Sigma}_t C_t^T + Q_t\right)^{-1} (CtΣˉtCtT+Qt)−1，我们可以用Matrix Inverse Lemma转化为：
( C t Σ ˉ t C t T + Q t ) − 1 = Q t − 1 − Q t − 1 C t ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 C t T Q t − 1 \left(C_t\bar{\Sigma}_t C_t^T + Q_t\right)^{-1} = Q_t^{-1} - Q_t^{-1}C_t\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}C_t^TQ_t^{-1} (CtΣˉtCtT+Qt)−1=Qt−1−Qt−1Ct(Σˉt−1+CtTQt−1Ct)−1CtTQt−1
那么 K t K_t Kt
K t = Σ ˉ t C t T ( C t Σ ˉ t C t T + Q t ) − 1 = Σ ˉ t C t T Q t − 1 − Σ ˉ t C t T Q t − 1 C t ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 C t T Q t − 1 = Σ ˉ t [ I − C t T Q t − 1 C t ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 ] C t T Q t − 1 = Σ ˉ t [ ( Σ ˉ t − 1 + C t T Q t − 1 C t ) ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 − C t T Q t − 1 C t ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 ] C t T Q t − 1 = ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 C t T Q t − 1 \begin{aligned} K_t &= \bar{\Sigma}_t C_t^T\left(C_t\bar{\Sigma}_t C_t^T + Q_t\right)^{-1} \\ &= \bar{\Sigma}_t C_t^T Q_t^{-1} - \bar{\Sigma}_t C_t^T Q_t^{-1}C_t\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}C_t^T Q_t^{-1} \\ &= \bar{\Sigma}_t\left[I - C_t^T Q_t^{-1}C_t\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}\right]C_t^T Q_t^{-1} \\ &= \bar{\Sigma}_t\left[\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1} - C_t^T Q_t^{-1}C_t\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}\right]C_t^T Q_t^{-1} \\ &= \left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}C_t^T Q_t^{-1} \end{aligned} Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1=ΣˉtCtTQt−1−ΣˉtCtTQt−1Ct(Σˉt−1+CtTQt−1Ct)−1CtTQt−1=Σˉt[I−CtTQt−1Ct(Σˉt−1+CtTQt−1Ct)−1]CtTQt−1=Σˉt[(Σˉt−1+CtTQt−1Ct)(Σˉt−1+CtTQt−1Ct)−1−CtTQt−1Ct(Σˉt−1+CtTQt−1Ct)−1]CtTQt−1=(Σˉt−1+CtTQt−1Ct)−1CtTQt−1
可以进一步计算更新过程中的信息矩阵：
Ω t − 1 = Σ t = ( I − K t C t ) Σ ˉ t = ( I − ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 C t T Q t − 1 C t ) Σ ˉ t = ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 = ( Ω ˉ t + C t T Q t − 1 C t ) − 1 ⇒ Ω t = Ω ˉ t + C t T Q t − 1 C t \begin{aligned} \Omega_t^{-1} &= \Sigma_t = \left(I - K_tC_t\right)\bar{\Sigma}_t \\ &= \left(I - \left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}C_t^T Q_t^{-1} C_t \right)\bar{\Sigma}_t \\ &= \left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1} \\ &= \left(\bar{\Omega}_t + C_t^T Q_t^{-1}C_t\right)^{-1} \\ \Rightarrow\qquad\Omega_t &= \bar{\Omega}_t + C_t^T Q_t^{-1}C_t \end{aligned} Ωt−1⇒Ωt=Σt=(I−KtCt)Σˉt=(I−(Σˉt−1+CtTQt−1Ct)−1CtTQt−1Ct)Σˉt=(Σˉt−1+CtTQt−1Ct)−1=(Ωˉt+CtTQt−1Ct)−1=Ωˉt+CtTQt−1Ct
现在来计算更新过程中的信息向量：
Ω t − 1 ξ t = μ t = μ ˉ t + K t ( z t − C t μ ˉ t ) = ( I − K t C t ) μ ˉ t + K t z t = ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 Σ ˉ t − 1 μ ˉ + K t z t ⇒ ξ t = Ω t ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 Σ ˉ t − 1 μ ˉ + Ω t K t z t = Σ ˉ t − 1 μ ˉ t + Ω t K t z t = ξ ˉ t + Ω t ( Σ ˉ t − 1 + C t T Q t − 1 C t ) − 1 C t T Q t − 1 z t = C t T Q t − 1 z t + ξ ˉ t \begin{aligned} \Omega_t^{-1}\boldsymbol{\xi}_t &= \boldsymbol{\mu}_t = \bar{\boldsymbol{\mu}}_t + K_t\left(\mathbf{z}_t - C_t\bar{\boldsymbol{\mu}}_t\right) = \left(I - K_t C_t\right)\bar{\boldsymbol{\mu}}_t + K_t\mathbf{z}_t \\ &= \left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}\bar{\Sigma}_t^{-1}\bar{\boldsymbol{\mu}} + K_t\mathbf{z}_t\\ \Rightarrow\qquad\boldsymbol{\xi}_t &= \Omega_t\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}\bar{\Sigma}_t^{-1}\bar{\boldsymbol{\mu}} + \Omega_t K_t\mathbf{z}_t \\ &= \bar{\Sigma}_t^{-1}\bar{\boldsymbol{\mu}}_t + \Omega_t K_t\mathbf{z}_t \\ &= \bar{\boldsymbol{\xi}}_t + \Omega_t\left(\bar{\Sigma}_t^{-1} + C_t^T Q_t^{-1}C_t\right)^{-1}C_t^T Q_t^{-1}\mathbf{z}_t \\ &= C_t^T Q_t^{-1}\mathbf{z}_t + \bar{\boldsymbol{\xi}}_t \end{aligned} Ωt−1ξt⇒ξt=μt=μˉt+Kt(zt−Ctμˉt)=(I−KtCt)μˉt+Ktzt=(Σˉt−1+CtTQt−1Ct)−1Σˉt−1μˉ+Ktzt=Ωt(Σˉt−1+CtTQt−1Ct)−1Σˉt−1μˉ+ΩtKtzt=Σˉt−1μˉt+ΩtKtzt=ξˉt+Ωt(Σˉt−1+CtTQt−1Ct)−1CtTQt−1zt=CtTQt−1zt+ξˉt
至此，我们已经全部推导出信息滤波器的公式：
Predict
Ω ˉ = ( A t Ω t − 1 − 1 A t T + R t ) − 1 ξ ˉ t = Ω ˉ t ( A t Ω t − 1 − 1 ξ t − 1 + B t u t ) \begin{aligned} \bar{\Omega} &= \left(A_t\Omega_{t-1}^{-1}A_t^T + R_t\right)^{-1} \\ \bar{\boldsymbol{\xi}}_t &= \bar{\Omega}_t\left(A_t\Omega_{t-1}^{-1}\boldsymbol{\xi}_{t-1} + B_t\mathbf{u}_t\right) \end{aligned} Ωˉξˉt=(AtΩt−1−1AtT+Rt)−1=Ωˉt(AtΩt−1−1ξt−1+Btut)
Update
Ω t = Ω ˉ t + C t T Q t − 1 C t ξ t = C t T Q t − 1 z t + ξ ˉ t \begin{aligned} \Omega_t &= \bar{\Omega}_t + C_t^T Q_t^{-1}C_t \\ \boldsymbol{\xi}_t &= C_t^T Q_t^{-1}\mathbf{z}_t + \bar{\boldsymbol{\xi}}_t \end{aligned} Ωtξt=Ωˉt+CtTQt−1Ct=CtTQt−1zt+ξˉt