2. 科技
    3. 6 概率机器人 Probabilistic Robotics 信息滤波算法

    6 概率机器人 Probabilistic Robotics 信息滤波算法

    科技2024-04-10  91

    文章目录

    1 前言2 信息滤波器2.1 参数规范法2.2 信息滤波算法2.3 信息滤波公式推导2.4 实例 4 参考文献

    1 前言

    信息滤波与卡尔曼滤波具有相同的前提假设条件 状态转移函数必须是线性函数: x t = A t x t − 1 + B t u t + ϵ t x_t = A_t x_{t-1} + B_t u_t + \epsilon_t xt​=At​xt−1​+Bt​ut​+ϵt​测量函数必须是线性函数: z t = C t x t + δ t z_t = C_t x_t + \delta_t zt​=Ct​xt​+δt​初始的置信度函数服从正态分布: b ( x 0 ) ∼ N ( μ 0 , Σ 0 ) b(x_0) \sim N(\mu_0, \Sigma_0) b(x0​)∼N(μ0​,Σ0​) 信息滤波与卡尔曼滤波不同之处在于高斯分布的表达方式不同： 卡尔曼滤波的高斯分布用均值 μ \mu μ和方差 Σ \Sigma Σ表示信息滤波的高斯分布用信息向量 ξ \xi ξ和信息矩阵 Ω \Omega Ω表示 信息滤波与卡尔曼滤波其实具有对偶性，算法的计算复杂程度不一样，一个简单另一个就复杂，具体的要看实际情况

    2 信息滤波器

    2.1 参数规范法

    传统的高斯分布用均值 μ \mu μ和方差 Σ \Sigma Σ表示，而信息滤波的高斯分布用信息向量 ξ \xi ξ和信息矩阵 Ω \Omega Ω表示 Ω = Σ − 1 ξ = Σ − 1 μ ⇒ Σ = Ω − 1 μ = Ω − 1 ξ 注：方差信息矩阵 Σ 也叫做不确定度矩阵；信息矩阵 Ω 也叫做精确度矩阵 \begin{aligned} \Omega &= \Sigma^{-1}\\ \xi &= \Sigma^{-1} \mu \end{aligned} \Rightarrow \begin{aligned} \Sigma &= \Omega^{-1}\\ \mu&= \Omega^{-1} \xi \end{aligned}\\ \text{注：方差信息矩阵$\Sigma$也叫做不确定度矩阵；信息矩阵$\Omega$也叫做精确度矩阵} Ωξ​=Σ−1=Σ−1μ​⇒Σμ​=Ω−1=Ω−1ξ​注：方差信息矩阵Σ也叫做不确定度矩阵；信息矩阵Ω也叫做精确度矩阵有人会问这么转换的意义是什么？？？下面简单对传统的高斯分布的表达式做一下简单的变换： p ( x ) = d e t ( 2 π Σ ) − 1 2 exp ⁡ { − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) } = d e t ( 2 π Σ ) − 1 2 exp ⁡ { − 1 2 x T Σ − 1 x + x T Σ − 1 μ − 1 2 μ T Σ − 1 μ } = d e t ( 2 π Σ ) − 1 2 exp ⁡ { − 1 2 μ T Σ − 1 μ } exp ⁡ { − 1 2 x T Σ − 1 x + x T Σ − 1 μ } ↓ d e t ( 2 π Σ ) − 1 2 exp ⁡ { − 1 2 μ T Σ − 1 μ } 为常数 = η exp ⁡ { − 1 2 x T Σ − 1 x + x T Σ − 1 μ } ↓ 用信息矩阵和信息向量替换 = η exp ⁡ { − 1 2 x T Ω x + x T ξ } \begin{aligned} p(x) &= det(2\pi\Sigma)^{-\frac{1}{2}}\exp\left\{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\right\} \\ &= det(2\pi\Sigma)^{-\frac{1}{2}}\exp\left\{-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}\mu -\frac{1}{2}\mu^T\Sigma^{-1} \mu \right\} \\ &= det(2\pi\Sigma)^{-\frac{1}{2}} \exp\left\{ -\frac{1}{2}\mu^T\Sigma^{-1} \mu \right\} \exp\left\{-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}\mu \right\} \\ & \downarrow det(2\pi\Sigma)^{-\frac{1}{2}} \exp\left\{ -\frac{1}{2}\mu^T\Sigma^{-1} \mu \right\} \text{为常数}\\ &= \eta \exp\left\{-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}\mu \right\} \\ & \downarrow \text{用信息矩阵和信息向量替换}\\ &= \eta \exp\left\{-\frac{1}{2}x^T\Omega x + x^T\xi \right\} \end{aligned} p(x)​=det(2πΣ)−21​exp{−21​(x−μ)TΣ−1(x−μ)}=det(2πΣ)−21​exp{−21​xTΣ−1x+xTΣ−1μ−21​μTΣ−1μ}=det(2πΣ)−21​exp{−21​μTΣ−1μ}exp{−21​xTΣ−1x+xTΣ−1μ}↓det(2πΣ)−21​exp{−21​μTΣ−1μ}为常数=ηexp{−21​xTΣ−1x+xTΣ−1μ}↓用信息矩阵和信息向量替换=ηexp{−21​xTΩx+xTξ}​

    2.2 信息滤波算法

    这里直接给出信息滤波算法流程，为了与卡尔曼滤波对比，也列出卡尔曼滤波算法流程 信息滤波算法流程

    卡尔曼滤波算法流程

    两种方法的对比： 两种方法的输入的不同之处在于信息滤波是 ξ t − 1 , Ω t − 1 \xi_{t-1}, \Omega_{t-1} ξt−1​,Ωt−1​,卡尔曼滤波是 μ t − 1 , Σ t − 1 \mu_{t-1}, \Sigma_{t-1} μt−1​,Σt−1​,其实两者可以互换两种方法的输出的不同之处在于信息滤波是 ξ t , Ω t \xi_{t}, \Omega_{t} ξt​,Ωt​,卡尔曼滤波是 μ t , Σ t \mu_{t}, \Sigma_{t} μt​,Σt​,其实两者也可以互换算法均由两步组成：预测和测量更新信息滤波不用再计算卡尔曼增益

    2.3 信息滤波公式推导

    这里还是采用类比的方法给出信息滤波公式 信息滤波：                                                                                                       Step 1: 预测 { Ω ˉ t = ( A t Ω t − 1 − 1 A t T + R t ) − 1 ξ ˉ t = Ω ˉ t ( A t Ω t − 1 − 1 ξ t − 1 + B t u t ) Step 2: 测量更新 { Ω t = C t T Q t − 1 C t + Ω ˉ t ξ t = C t T Q t − 1 z t + ξ ˉ t ⇕ 类比 卡尔曼滤波：                                                                                                       Step 1: 预测 { Σ ˉ t = A t Σ t − 1 A t T + R t μ ˉ t = A t μ t − 1 + B t u t Step 2: 测量更新 { 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 \text{信息滤波：}~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\\ \text{Step 1: 预测}\left\{\begin{matrix} &\bar{\Omega}_t &= &\left (A_t\Omega^{-1}_{t-1} A_t^T+R_t \right )^{-1}\\ &\bar{\xi}_{t} &= &\bar{\Omega}_t \left ( A_t\Omega^{-1}_{t-1} \xi_{t-1} + B_t u_t \right ) \end{matrix}\right.\\ \text{Step 2: 测量更新}\left\{\begin{matrix} &\Omega_t &= & C_t^TQ_t^{-1}C_t + \bar{\Omega}_t\\ &\xi_t &= & C_t^TQ_t^{-1}z_t + \bar{\xi}_{t}\\ \end{matrix}\right.\\ \Updownarrow \text{类比}\\ \text{卡尔曼滤波：}~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\\ \text{Step 1: 预测}\left\{\begin{matrix} \bar{\Sigma}_t = A_t\Sigma_{t-1} A_t^T+R_t\\ \bar{\mu}_t = A_t\mu_{t-1}+B_t u_t\\ \end{matrix}\right.\\ \text{Step 2: 测量更新}\left\{\begin{matrix} K_t =\bar{\Sigma}_t C_t^T(C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}\\ \mu_t =\bar{\mu}_t + K_t(z_t - C_t\bar{\mu}_t)\\ \Sigma_t =(I - K_tC_t)\bar{\Sigma}_t \end{matrix}\right. 信息滤波：                                                                                                      Step 1: 预测{​Ωˉt​ξˉ​t​​==​(At​Ωt−1−1​AtT​+Rt​)−1Ωˉt​(At​Ωt−1−1​ξt−1​+Bt​ut​)​Step 2: 测量更新{​Ωt​ξt​​==​CtT​Qt−1​Ct​+Ωˉt​CtT​Qt−1​zt​+ξˉ​t​​⇕类比卡尔曼滤波：                                                                                                      Step 1: 预测{Σˉt​=At​Σt−1​AtT​+Rt​μˉ​t​=At​μt−1​+Bt​ut​​Step 2: 测量更新⎩⎨⎧​Kt​=Σˉt​CtT​(Ct​Σˉt​CtT​+Qt​)−1μt​=μˉ​t​+Kt​(zt​−Ct​μˉ​t​)Σt​=(I−Kt​Ct​)Σˉt​​需要的替换公式： Ω = Σ − 1 ξ = Σ − 1 μ ⇔ Σ = Ω − 1 μ = Ω − 1 ξ ↓ Ω ˉ t = Σ ˉ t − 1 ξ ˉ t = Σ ˉ t − 1 μ ˉ t ⇔ Σ ˉ t = Ω ˉ t − 1 μ ˉ t = Ω ˉ t − 1 ξ ˉ t        Ω t − 1 = Σ t − 1 − 1 ξ t − 1 = Σ t − 1 − 1 μ t − 1 ⇔ Σ t − 1 = Ω t − 1 − 1 μ t − 1 = Ω t − 1 − 1 ξ t − 1        Ω t = Σ t − 1 ξ t = Σ t − 1 μ t ⇔ Σ t = Ω t − 1 μ t = Ω t − 1 ξ t \begin{aligned} \Omega &= \Sigma^{-1}\\ \xi &= \Sigma^{-1} \mu \end{aligned} \Leftrightarrow \begin{aligned} \Sigma &= \Omega^{-1}\\ \mu&= \Omega^{-1} \xi \end{aligned}\\ \downarrow\\ \begin{aligned} \bar{\Omega}_{t} &= \bar{\Sigma}_{t}^{-1}\\ \bar{\xi}_{t} &= \bar{\Sigma}_{t}^{-1} \bar{\mu}_{t} \end{aligned} \Leftrightarrow \begin{aligned} \bar{\Sigma}_{t} &= \bar{\Omega}_{t}^{-1}\\ \bar{\mu}_{t} &= \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} \end{aligned} \begin{aligned}~~~~~~ \Omega_{t-1} &= \Sigma_{t-1}^{-1}\\ \xi_{t-1} &= \Sigma_{t-1}^{-1} \mu_{t-1} \end{aligned} \Leftrightarrow \begin{aligned} \Sigma_{t-1} &= \Omega_{t-1}^{-1}\\ \mu_{t-1} &= \Omega_{t-1}^{-1} \xi_{t-1} \end{aligned}~~~~~~ \begin{aligned} \Omega_{t} &= \Sigma_{t}^{-1}\\ \xi_{t} &= \Sigma_{t}^{-1} \mu_{t} \end{aligned} \Leftrightarrow \begin{aligned} \Sigma_{t} &= \Omega_{t}^{-1}\\ \mu_{t} &= \Omega_{t}^{-1} \xi_{t} \end{aligned} Ωξ​=Σ−1=Σ−1μ​⇔Σμ​=Ω−1=Ω−1ξ​↓Ωˉt​ξˉ​t​​=Σˉt−1​=Σˉt−1​μˉ​t​​⇔Σˉt​μˉ​t​​=Ωˉt−1​=Ωˉt−1​ξˉ​t​​      Ωt−1​ξt−1​​=Σt−1−1​=Σt−1−1​μt−1​​⇔Σt−1​μt−1​​=Ωt−1−1​=Ωt−1−1​ξt−1​​      Ωt​ξt​​=Σt−1​=Σt−1​μt​​⇔Σt​μt​​=Ωt−1​=Ωt−1​ξt​​预测步公式推导 卡尔曼滤波： Σ ˉ t = A t Σ t − 1 A t T + R t ↓ Ω ˉ t − 1 = A t Ω t − 1 − 1 A t T + R t ↓ Ω ˉ t = ( A t Ω t − 1 − 1 A t T + R 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 ) Step 1: 预测 { Ω ˉ t = ( A t Ω t − 1 − 1 A t T + R t ) − 1 ξ ˉ t = Ω ˉ t ( A t Ω t − 1 − 1 ξ t − 1 + B t u t ) \text{卡尔曼滤波：}\bar{\Sigma}_t = A_t\Sigma_{t-1} A_t^T+R_t\\ \downarrow\\ \bar{\Omega}_{t}^{-1} = A_t\Omega_{t-1}^{-1}A_t^T+R_t\\ \downarrow\\ \bar{\Omega}_{t} = (A_t\Omega_{t-1}^{-1}A_t^T+R_t)^{-1}\\ \text{卡尔曼滤波：}\bar{\mu}_t = A_t\mu_{t-1}+B_t u_t\\ \downarrow\\ \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} = A_t \Omega_{t-1}^{-1} \xi_{t-1} +B_t u_t\\ \downarrow\\ \bar{\xi }_{t} = \bar{\Omega}_{t} (A_t \Omega_{t-1}^{-1} \xi_{t-1} +B_t u_t)\\ \text{Step 1: 预测}\left\{\begin{matrix} &\bar{\Omega}_t &= &\left (A_t\Omega^{-1}_{t-1} A_t^T+R_t \right )^{-1}\\ &\bar{\xi}_{t} &= &\bar{\Omega}_t \left ( A_t\Omega^{-1}_{t-1} \xi_{t-1} + B_t u_t \right ) \end{matrix}\right. 卡尔曼滤波：Σˉt​=At​Σt−1​AtT​+Rt​↓Ωˉt−1​=At​Ωt−1−1​AtT​+Rt​↓Ωˉt​=(At​Ωt−1−1​AtT​+Rt​)−1卡尔曼滤波：μˉ​t​=At​μt−1​+Bt​ut​↓Ωˉt−1​ξˉ​t​=At​Ωt−1−1​ξt−1​+Bt​ut​↓ξˉ​t​=Ωˉt​(At​Ωt−1−1​ξt−1​+Bt​ut​)Step 1: 预测{​Ωˉt​ξˉ​t​​==​(At​Ωt−1−1​AtT​+Rt​)−1Ωˉt​(At​Ωt−1−1​ξt−1​+Bt​ut​)​测量更新公式推导 在推导卡尔曼滤波时，得到与测量更新步骤等价的公式 { K t = Σ t C t T Q t − 1 μ t = μ ˉ t + K t ( z t − C t μ ˉ t ) Σ t = ( C t T Q t − 1 C t + Σ ˉ t − 1 ) − 1 ⇔ { 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 \left\{\begin{matrix} &K_t&=&\Sigma_t C_t^TQ_t^{-1}\\ &\mu_t &= &\bar{\mu}_t + K_t(z_t - C_t\bar{\mu}_t)\\ &\Sigma_t &= &(C_t^TQ_t^{-1}C_t + \bar{\Sigma}_t^{-1})^{-1} \end{matrix}\right. \Leftrightarrow \left\{\begin{matrix} &K_t &=&\bar{\Sigma}_t C_t^T(C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}\\ &\mu_t &=&\bar{\mu}_t + K_t(z_t - C_t\bar{\mu}_t)\\ &\Sigma_t &=&(I - K_tC_t)\bar{\Sigma}_t \end{matrix}\right. ⎩⎨⎧​​Kt​μt​Σt​​===​Σt​CtT​Qt−1​μˉ​t​+Kt​(zt​−Ct​μˉ​t​)(CtT​Qt−1​Ct​+Σˉt−1​)−1​⇔⎩⎨⎧​​Kt​μt​Σt​​===​Σˉt​CtT​(Ct​Σˉt​CtT​+Qt​)−1μˉ​t​+Kt​(zt​−Ct​μˉ​t​)(I−Kt​Ct​)Σˉt​​ 我们这次用左侧式子推导（左右两组式子的等价的） 卡尔曼滤波： Σ t = ( C t T Q t − 1 C t + Σ ˉ t − 1 ) − 1 ↓ Ω t − 1 = ( C t T Q t − 1 C t + Ω ˉ t ) − 1 ↓ Ω t = C t T Q t − 1 C t + Ω ˉ t 卡尔曼滤波： μ t = μ ˉ t + K t ( z t − C t μ ˉ t ) ↓ μ t = Ω t − 1 ξ t ↓ μ ˉ t = Ω ˉ t − 1 ξ ˉ t ↓ K t = Σ t C t T Q t − 1 Ω t − 1 ξ t = Ω ˉ t − 1 ξ ˉ t + Σ t C t T Q t − 1 ( z t − C t Ω ˉ t − 1 ξ ˉ t ) ↓ Σ t = Ω t − 1 ξ t = Ω t Ω ˉ t − 1 ξ ˉ t + C t T Q t − 1 ( z t − C t Ω ˉ t − 1 ξ ˉ t ) ↓ ξ t = Ω t Ω ˉ t − 1 ξ ˉ t + C t T Q t − 1 z t − C t T Q t − 1 C t Ω ˉ t − 1 ξ ˉ t ↓ ξ t = C t T Q t − 1 z t + Ω t Ω ˉ t − 1 ξ ˉ t − C t T Q t − 1 C t Ω ˉ t − 1 ξ ˉ t ↓ Ω t = C t T Q t − 1 C t + Ω ˉ t ξ t = C t T Q t − 1 z t + ( C t T Q t − 1 C t + Ω ˉ t ) Ω ˉ t − 1 ξ ˉ t − C t T Q t − 1 C t Ω ˉ t − 1 ξ ˉ t ↓ 化简 ξ t = C t T Q t − 1 z t + ξ ˉ t Step 2: 测量更新 { Ω t = C t T Q t − 1 C t + Ω ˉ t ξ t = C t T Q t − 1 z t + ξ ˉ t \text{卡尔曼滤波：} \Sigma_t = (C_t^TQ_t^{-1}C_t + \bar{\Sigma}_t^{-1})^{-1}\\ \downarrow\\ \Omega_t^{-1} = (C_t^TQ_t^{-1}C_t + \bar{\Omega}_{t} )^{-1} \\ \downarrow\\ \Omega_t = C_t^TQ_t^{-1}C_t + \bar{\Omega}_{t} \\ \text{卡尔曼滤波：} \mu_t = \bar{\mu}_t + K_t(z_t - C_t\bar{\mu}_t)\\ \downarrow \mu_{t} = \Omega_{t}^{-1} \xi_{t}\\ \downarrow \bar{\mu}_{t} = \bar{\Omega}_{t}^{-1} \bar{\xi }_{t}\\ \downarrow K_t=\Sigma_t C_t^TQ_t^{-1}\\ \Omega_{t}^{-1} \xi_{t} = \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} + \Sigma_t C_t^TQ_t^{-1}(z_t - C_t \bar{\Omega}_{t}^{-1} \bar{\xi }_{t})\\ \downarrow \Sigma_{t} = \Omega_{t}^{-1}\\ \xi_{t} = \Omega_{t} \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} + C_t^TQ_t^{-1}(z_t - C_t \bar{\Omega}_{t}^{-1} \bar{\xi }_{t})\\ \downarrow \\ \xi_{t} = \Omega_{t} \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} + C_t^TQ_t^{-1}z_t - C_t^TQ_t^{-1}C_t \bar{\Omega}_{t}^{-1} \bar{\xi }_{t}\\ \downarrow \\ \xi_{t} = C_t^TQ_t^{-1}z_t + \Omega_{t} \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} - C_t^TQ_t^{-1}C_t \bar{\Omega}_{t}^{-1} \bar{\xi }_{t}\\ \downarrow \Omega_t = C_t^TQ_t^{-1}C_t + \bar{\Omega}_{t} \\ \xi_{t} = C_t^TQ_t^{-1}z_t + (C_t^TQ_t^{-1}C_t + \bar{\Omega}_{t} ) \bar{\Omega}_{t}^{-1} \bar{\xi }_{t} - C_t^TQ_t^{-1}C_t \bar{\Omega}_{t}^{-1} \bar{\xi }_{t}\\ \downarrow \text{化简}\\ \xi_{t} = C_t^TQ_t^{-1}z_t + \bar{\xi }_{t} \\ \text{Step 2: 测量更新}\left\{\begin{matrix} &\Omega_t &= & C_t^TQ_t^{-1}C_t + \bar{\Omega}_t\\ &\xi_t &= & C_t^TQ_t^{-1}z_t + \bar{\xi}_{t}\\ \end{matrix}\right. 卡尔曼滤波：Σt​=(CtT​Qt−1​Ct​+Σˉt−1​)−1↓Ωt−1​=(CtT​Qt−1​Ct​+Ωˉt​)−1↓Ωt​=CtT​Qt−1​Ct​+Ωˉt​卡尔曼滤波：μt​=μˉ​t​+Kt​(zt​−Ct​μˉ​t​)↓μt​=Ωt−1​ξt​↓μˉ​t​=Ωˉt−1​ξˉ​t​↓Kt​=Σt​CtT​Qt−1​Ωt−1​ξt​=Ωˉt−1​ξˉ​t​+Σt​CtT​Qt−1​(zt​−Ct​Ωˉt−1​ξˉ​t​)↓Σt​=Ωt−1​ξt​=Ωt​Ωˉt−1​ξˉ​t​+CtT​Qt−1​(zt​−Ct​Ωˉt−1​ξˉ​t​)↓ξt​=Ωt​Ωˉt−1​ξˉ​t​+CtT​Qt−1​zt​−CtT​Qt−1​Ct​Ωˉt−1​ξˉ​t​↓ξt​=CtT​Qt−1​zt​+Ωt​Ωˉt−1​ξˉ​t​−CtT​Qt−1​Ct​Ωˉt−1​ξˉ​t​↓Ωt​=CtT​Qt−1​Ct​+Ωˉt​ξt​=CtT​Qt−1​zt​+(CtT​Qt−1​Ct​+Ωˉt​)Ωˉt−1​ξˉ​t​−CtT​Qt−1​Ct​Ωˉt−1​ξˉ​t​↓化简ξt​=CtT​Qt−1​zt​+ξˉ​t​Step 2: 测量更新{​Ωt​ξt​​==​CtT​Qt−1​Ct​+Ωˉt​CtT​Qt−1​zt​+ξˉ​t​​ 综上所述 Step 1: 预测 { Ω ˉ t = ( A t Ω t − 1 − 1 A t T + R t ) − 1 ξ ˉ t = Ω ˉ t ( A t Ω t − 1 − 1 ξ t − 1 + B t u t ) Step 2: 测量更新 { Ω t = C t T Q t − 1 C t + Ω ˉ t ξ t = C t T Q t − 1 z t + ξ ˉ t \text{Step 1: 预测}\left\{\begin{matrix} &\bar{\Omega}_t &= &\left (A_t\Omega^{-1}_{t-1} A_t^T+R_t \right )^{-1}\\ &\bar{\xi}_{t} &= &\bar{\Omega}_t \left ( A_t\Omega^{-1}_{t-1} \xi_{t-1} + B_t u_t \right ) \end{matrix}\right.\\ \text{Step 2: 测量更新}\left\{\begin{matrix} &\Omega_t &= & C_t^TQ_t^{-1}C_t + \bar{\Omega}_t\\ &\xi_t &= & C_t^TQ_t^{-1}z_t + \bar{\xi}_{t}\\ \end{matrix}\right. Step 1: 预测{​Ωˉt​ξˉ​t​​==​(At​Ωt−1−1​AtT​+Rt​)−1Ωˉt​(At​Ωt−1−1​ξt−1​+Bt​ut​)​Step 2: 测量更新{​Ωt​ξt​​==​CtT​Qt−1​Ct​+Ωˉt​CtT​Qt−1​zt​+ξˉ​t​​

    2.4 实例

    信息滤波的例子采用与卡尔曼滤波相同的例子：测量房间温度测量房间温度 目标：估计 t t t时刻房间的温度 x t x_t xt​条件： 经验判断房间温度恒定 A t = 1 A_t=1 At​=1这个经验不确定度为 4 ° C 4\degree C 4°C即方差 R t = 4 2 = 16 R_t=4^2=16 Rt​=42=16,不采取任何控制动作 B t = 0 B_t=0 Bt​=0那么状态转移概率：感知的温度 x ˉ t ∼ N ( A t x t − 1 + B t u t , R t ) ∼ N ( x t − 1 , 16 ) \bar{x}_t \sim N(A_t x_{t-1} + B_t u_t, R_t) \sim N(x_{t-1}, 16) xˉt​∼N(At​xt−1​+Bt​ut​,Rt​)∼N(xt−1​,16)房间还有一个温度计，可以直接测出摄氏度温度 C t = 1 C_t = 1 Ct​=1温度计不确定度也为 4 ° C 4\degree C 4°C即方差 Q t = 4 2 = 16 Q_t=4^2=16 Qt​=42=16测量函数概率：温度计测量的温度 z t ∼ N ( C t x ^ t , Q t ) ∼ N ( x ^ t , 16 ) z_t \sim N(C_t \hat{x}_t, Q_t) \sim N(\hat{x}_t, 16) zt​∼N(Ct​x^t​,Qt​)∼N(x^t​,16) 滤波过程： t = 0 t=0 t=0时刻，已知条件： μ 0 = 23 , Σ 0 = 3 2 = 9 \mu_0=23, \Sigma_0=3^2=9 μ0​=23,Σ0​=32=9即 Ω 0 = 1 9 , ξ 0 = 23 9 \Omega_0 = \frac{1}{9}, \xi_0 = \frac{23}{9} Ω0​=91​,ξ0​=923​ t = 1 t=1 t=1时刻: 温度计测量值为 25 ° C 25\degree C 25°C,即 z 1 = 25 z_1=25 z1​=25 line 2: Ω ˉ 1 = ( A 1 Ω 0 − 1 A 1 T + R 1 ) − 1 = ( 9 + 16 ) − 1 = 1 25 \bar{\Omega}_1 = \left (A_1\Omega^{-1}_{0} A_1^T+R_1 \right )^{-1} = (9+16)^{-1}=\frac{1}{25} Ωˉ1​=(A1​Ω0−1​A1T​+R1​)−1=(9+16)−1=251​line 3: ξ ˉ 1 = Ω ˉ 1 ( A 1 Ω 0 − 1 ξ 0 + B 1 u 1 ) = 23 25 \bar{\xi}_{1} = \bar{\Omega}_1 \left ( A_1\Omega^{-1}_{0} \xi_{0} + B_1 u_1 \right ) = \frac{23}{25} ξˉ​1​=Ωˉ1​(A1​Ω0−1​ξ0​+B1​u1​)=2523​line 4: Ω 1 = C 1 T Q 1 − 1 C 1 + Ω ˉ 1 = 1 16 + 1 25 = 9.7561 \Omega_1 = C_1^TQ_1^{-1}C_1 + \bar{\Omega}_1 = \frac{1}{16} + \frac{1}{25} = 9.7561 Ω1​=C1T​Q1−1​C1​+Ωˉ1​=161​+251​=9.7561line 5: ξ 1 = C 1 T Q 1 − 1 z 1 + ξ ˉ 1 = 25 16 + 23 25 = 2.4825 \xi_1 = C_1^TQ_1^{-1}z_1 + \bar{\xi}_{1} = \frac{25}{16} + \frac{23}{25} = 2.4825 ξ1​=C1T​Q1−1​z1​+ξˉ​1​=1625​+2523​=2.4825同理，有了 Ω 1 , ξ 1 \Omega_1 ,\xi_1 Ω1​,ξ1​，继续循环计算。。。

    4 参考文献

    Thrun, & Sebastian. (2005). Probabilistic robotics.
    Processed: 0.015, SQL: 8