网站建设968,WordPress的插件怎么保存,玉林住房和城乡建设部网站,建设网络道德教育网站的有效措施多传感器融合定位十-基于滤波的融合方法Ⅰ其二3. 滤波器基本原理3.1 状态估计模型3.2 贝叶斯滤波3.3 卡尔曼滤波(KF)推导3.4 扩展卡尔曼滤波(EKF)推导3.5 迭代扩展卡尔曼滤波(IEKF)推导4. 基于滤波器的融合4.1 状态方程4.2 观测方程4.3 构建滤波器4.4 Kalman 滤波实际使用流程4…
多传感器融合定位十-基于滤波的融合方法Ⅰ其二3. 滤波器基本原理3.1 状态估计模型3.2 贝叶斯滤波3.3 卡尔曼滤波(KF)推导3.4 扩展卡尔曼滤波(EKF)推导3.5 迭代扩展卡尔曼滤波(IEKF)推导4. 基于滤波器的融合4.1 状态方程4.2 观测方程4.3 构建滤波器4.4 Kalman 滤波实际使用流程4.4.1 位姿初始化4.4.2 Kalman 初始化4.4.3 惯性解算4.4.4 Kalman 预测更新4.4.5 无观测时后验更新4.4.6 有观测时的量测更新4.4.7 有观测时计算后验位姿4.4.8 有观测时状态量清零4.4.9 输出位姿Reference:
深蓝学院-多传感器融合多传感器融合定位理论基础
文章跳转
多传感器融合定位一-3D激光里程计其一ICP多传感器融合定位二-3D激光里程计其二NDT多传感器融合定位三-3D激光里程计其三点云畸变补偿多传感器融合定位四-3D激光里程计其四点云线面特征提取多传感器融合定位五-点云地图构建及定位多传感器融合定位六-惯性导航原理及误差分析多传感器融合定位七-惯性导航解算及误差分析其一多传感器融合定位八-惯性导航解算及误差分析其二多传感器融合定位九-基于滤波的融合方法Ⅰ其一多传感器融合定位十-基于滤波的融合方法Ⅰ其二多传感器融合定位十一-基于滤波的融合方法Ⅱ多传感器融合定位十二-基于图优化的建图方法其一多传感器融合定位十三-基于图优化的建图方法其二多传感器融合定位十四-基于图优化的定位方法多传感器融合定位十五-多传感器时空标定(综述)
3. 滤波器基本原理
3.1 状态估计模型
实际状态估计任务中待估计的后验概率密度可以表示为 p(xk∣xˇ0,v1:k,y0:k)p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) p(xk∣xˇ0,v1:k,y0:k)其中 xˇ0\check{\boldsymbol{x}}_0xˇ0 表示的是状态初始值 v1:k\boldsymbol{v}_{1: k}v1:k 表示从 111 到 kkk 时刻的输入 y0:k\boldsymbol{y}_{0: k}y0:k 表示从 000 到 kkk 时刻的观测 因此滤波问题可以直观表示为根据所有历史数据(输入、观测、初始状态)得出最终的融合结果。
历史数据之间的关系可以用下面的图模型表示 图模型中体现了马尔可夫性即当前状态只跟前一时刻状态相关和其他历史时刻状态无关。 该性质的数学表达 运动方程xkf(xk−1,vk,wk)\boldsymbol{x}_k\boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_k, \boldsymbol{w}_k\right)xkf(xk−1,vk,wk) 观测方程ykg(xk,nk)\boldsymbol{y}_k\boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right)ykg(xk,nk)
3.2 贝叶斯滤波
公式的推导可参考非线性优化 根据贝叶斯公式kkk 时刻后验概率密度可以表示为 p(xk∣xˇ0,v1:k,y0:k)p(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)p(yk∣xˇ0,v1:k,y0:k−1)ηp(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)\begin{aligned} p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) \frac{p\left(\boldsymbol{y}_k \mid \boldsymbol{x}_k, \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)}{p\left(\boldsymbol{y}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)} \\ \eta p\left(\boldsymbol{y}_k \mid \boldsymbol{x}_k, \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \end{aligned} p(xk∣xˇ0,v1:k,y0:k)p(yk∣xˇ0,v1:k,y0:k−1)p(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)ηp(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)(这里 yk\boldsymbol{y_k}yk 是当前时刻的观测而 p(xk∣xˇ0,v1:k,y0:k)p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)p(xk∣xˇ0,v1:k,y0:k) 是当前时刻后验p(xk∣xˇ0,v1:k,y0:k−1)p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)p(xk∣xˇ0,v1:k,y0:k−1)为先验。我们需要的是后验概率最大化因为贝叶斯分母部分与待估计的状态无关因而可以忽略)
根据观测方程 yk\boldsymbol{y}_kyk 只和 xk\boldsymbol{x}_kxk 相关因此上式可以简写为 p(xk∣xˇ0,v1:k,y0:k)ηp(yk∣xk)p(xk∣xˇ0,v1:k,y0:k−1)p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)\eta p\left(\boldsymbol{y}_k \mid \boldsymbol{x}_k\right) p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p(xk∣xˇ0,v1:k,y0:k)ηp(yk∣xk)p(xk∣xˇ0,v1:k,y0:k−1)利用条件分布的性质可得 p(xk∣xˇ0,v1:k,y0:k−1)∫p(xk∣xk−1,xˇ0,v1:k,y0:k−1)p(xk−1∣xˇ0,v1:k,y0:k−1)dxk−1\begin{aligned} p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \\ \int p\left(\boldsymbol{x}_k \mid \boldsymbol{x}_{k-1}, \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1} \end{aligned} p(xk∣xˇ0,v1:k,y0:k−1)∫p(xk∣xk−1,xˇ0,v1:k,y0:k−1)p(xk−1∣xˇ0,v1:k,y0:k−1)dxk−1再利用马尔可夫性可得 p(xk∣xˇ0,v1:k,y0:k−1)∫p(xk∣xk−1,vk)p(xk−1∣xˇ0,v1:k,y0:k−1)dxk−1\begin{aligned} p\left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \\ \int p\left(\boldsymbol{x}_k \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_k\right) p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1} \end{aligned} p(xk∣xˇ0,v1:k,y0:k−1)∫p(xk∣xk−1,vk)p(xk−1∣xˇ0,v1:k,y0:k−1)dxk−1经过以上化简最终后验概率可以写为 根据以上结果可以画出贝叶斯滤波的信息流图如下 贝叶斯滤波是一个非常广泛的概念它不特指某一种滤波
在高斯假设前提下用贝叶斯滤波的原始形式推导比较复杂可以利用高斯的特征得到简化形式即广义高斯滤波。后面 KF、EKF、IEKF 的推导均采用这种形式。实际中UKF 和 PF 多应用于扫地机器人等2D小场景与本课程目标场景不符因此不做讲解。UKF 和 PF 本身有一个维度的问题维度高了不太行而我们这里使用的维度多半是15维的在三维场景就不好用了
3.3 卡尔曼滤波(KF)推导
在线性高斯假设下上式可以重新写为下面的形式(为了和后面符号对应) 运动方程: xkF(xk−1,vk)Bk−1wk\boldsymbol{x}_k\boldsymbol{F}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_k\right)\boldsymbol{B}_{k-1} \boldsymbol{w}_kxkF(xk−1,vk)Bk−1wk 观测方程: ykG(xk)Cknk\boldsymbol{y}_k\boldsymbol{G}\left(\boldsymbol{x}_k\right)\boldsymbol{C}_k \boldsymbol{n}_kykG(xk)Cknk (F\boldsymbol{F}F 和 G\boldsymbol{G}G 在这里代表的是线性的意思非线性是后面要推导的。这里的 G\boldsymbol{G}G 和之前卡尔曼文章里写的 H\boldsymbol{H}H 是一个东西。n\boldsymbol{n}n 为观测噪声是个零均值白噪声) 把上一时刻的后验状态写为 p(xk−1∣xˇ0,v1:k−1,y0:k−1)N(x^k−1,P^k−1)p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right)\mathcal{N}\left(\hat{\boldsymbol{x}}_{k-1}, \hat{\boldsymbol{P}}_{k-1}\right) p(xk−1∣xˇ0,v1:k−1,y0:k−1)N(x^k−1,P^k−1)则当前时刻的预测值为 xˇkF(x^k−1,vk)\check{\boldsymbol{x}}_k\boldsymbol{F}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_k\right) xˇkF(x^k−1,vk)根据高斯分布的线性变化它的方差为(可仿照第2.8节中的推导过程自行推导) PˇkFk−1P^k−1Fk−1TBk−1QkBk−1T\check{\boldsymbol{P}}_k\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}\boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^{\mathrm{T}} PˇkFk−1P^k−1Fk−1TBk−1QkBk−1T其中 QkQ_kQk 为当前输入噪声的方差。
若把 kkk 时刻状态和观测的联合高斯分布写为 p(xk,yk∣xˇ0,v1:k,y0:k−1)N([μx,kμy,k],[Σxx,kΣxy,kΣyx,kΣyy,k])p\left(\boldsymbol{x}_k, \boldsymbol{y}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)\mathcal{N}\left(\left[\begin{array}{c} \boldsymbol{\mu}_{x, k} \\ \boldsymbol{\mu}_{y, k} \end{array}\right],\left[\begin{array}{cc} \boldsymbol{\Sigma}_{x x, k} \boldsymbol{\Sigma}_{x y, k} \\ \boldsymbol{\Sigma}_{y x, k} \boldsymbol{\Sigma}_{y y, k} \end{array}\right]\right) p(xk,yk∣xˇ0,v1:k,y0:k−1)N([μx,kμy,k],[Σxx,kΣyx,kΣxy,kΣyy,k])根据第2.7节中的推导结果 kkk 时刻的后验概率可以写为 p(xk∣xˇ0,v1:k,y0:k)N(μx,kΣxy,kΣyy,k−1(yk−μy,k)⏟x^k,Σxx,k−Σxy,kΣyy,k−1Σyx,k⏟P^k)\begin{aligned} p \left(\boldsymbol{x}_k \mid \check{\boldsymbol{x}}_0, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) \\ \mathcal{N}(\underbrace{\boldsymbol{\mu}_{x, k}\boldsymbol{\Sigma}_{x y, k} \boldsymbol{\Sigma}_{y y, k}^{-1}\left(\boldsymbol{y}_k-\boldsymbol{\mu}_{y, k}\right)}_{\hat{\boldsymbol{x}}_k}, \underbrace{\boldsymbol{\Sigma}_{x x, k}-\boldsymbol{\Sigma}_{x y, k} \boldsymbol{\Sigma}_{y y, k}^{-1} \boldsymbol{\Sigma}_{y x, k}}_{\hat{P}_k}) \end{aligned} p(xk∣xˇ0,v1:k,y0:k)N(x^kμx,kΣxy,kΣyy,k−1(yk−μy,k),P^kΣxx,k−Σxy,kΣyy,k−1Σyx,k)其中 x^k\hat{\boldsymbol{x}}_kx^k 和 P^k\hat{\boldsymbol{P}}_kP^k 分别为后验均值和方差。若定义 KkΣxy,kΣyy,k−1\boldsymbol{K}_k\boldsymbol{\Sigma}_{x y, k} \boldsymbol{\Sigma}_{y y, k}^{-1} KkΣxy,kΣyy,k−1则有 P^kPˇk−KkΣxy,kTx^kxˇkKk(yk−μy,k)\begin{aligned} \hat{\boldsymbol{P}}_k\check{\boldsymbol{P}}_k-\boldsymbol{K}_k \boldsymbol{\Sigma}_{x y, k}^{\mathrm{T}} \\ \hat{\boldsymbol{x}}_k\check{\boldsymbol{x}}_k\boldsymbol{K}_k\left(\boldsymbol{y}_k-\boldsymbol{\mu}_{y, k}\right) \end{aligned} P^kPˇk−KkΣxy,kTx^kxˇkKk(yk−μy,k)把第2.8节中的推导得出的线性变换后的均值、方差及交叉项带入上面的式子可以得到 KkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(1−KkGk)Pˇkx^kxˇkKk(yk−G(xˇk))\begin{aligned} \boldsymbol{K}_k \check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\left(\boldsymbol{G}_k \check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\boldsymbol{C}_k \boldsymbol{R}_k \boldsymbol{C}_k^{\mathrm{T}}\right)^{-1} \\ \hat{\boldsymbol{P}}_k \left(\mathbf{1}-\boldsymbol{K}_k \boldsymbol{G}_k\right) \check{\boldsymbol{P}}_k \\ \hat{\boldsymbol{x}}_k \check{\boldsymbol{x}}_k\boldsymbol{K}_k\left(\boldsymbol{y}_k-\boldsymbol{G}\left(\check{\boldsymbol{x}}_k\right)\right) \end{aligned} KkP^kx^kPˇkGkT(GkPˇkGkTCkRkCkT)−1(1−KkGk)PˇkxˇkKk(yk−G(xˇk))上面方程与之前所述预测方程(如下)就构成了卡尔曼经典五个方程。 xˇkF(x^k−1,vk)PˇkFk−1P^k−1Fk−1TBk−1QkBk−1T\begin{gathered} \check{\boldsymbol{x}}_k\boldsymbol{F}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_k\right) \\ \check{\boldsymbol{P}}_k\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}\boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^{\mathrm{T}} \end{gathered} xˇkF(x^k−1,vk)PˇkFk−1P^k−1Fk−1TBk−1QkBk−1T需要说明的是若不把第2.8节中的结果带入而保留上页的原始形式则对应的五个方程被称为广义高斯滤波。
3.4 扩展卡尔曼滤波(EKF)推导
当运动方程或观测方程为非线性的时候无法再利用之前所述的线性变化关系进行推导常用的解决方法是进行线性化把非线性方程一阶泰勒展开成线性。即 运动方程: xkf(xk−1,vk,wk)≈xˇkFk−1(xk−1−x^k−1)Bk−1wk\boldsymbol{x}_k\boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_k, \boldsymbol{w}_k\right) \approx \check{\boldsymbol{x}}_k\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)\boldsymbol{B}_{k-1} \boldsymbol{w}_kxkf(xk−1,vk,wk)≈xˇkFk−1(xk−1−x^k−1)Bk−1wk 观测方程: ykg(xk,nk)≈yˇkGk(xk−xˇk)Cknk\boldsymbol{y}_k\boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right) \approx \check{\boldsymbol{y}}_k\boldsymbol{G}_k\left(\boldsymbol{x}_k-\check{\boldsymbol{x}}_k\right)\boldsymbol{C}_k \boldsymbol{n}_kykg(xk,nk)≈yˇkGk(xk−xˇk)Cknk 其中 xˇkf(x^k−1,vk,0)yˇkg(xˇk,0)Fk−1∂f(xk−1,vk,wk)∂xk−1∣x^k−1,vk,0Gk∂g(xk,nk)∂xk∣xˇk,0Bk−1∂f(xk−1,vk,wk)∂wk∣x^k−1,vk,0Ck∂g(xk,nk)∂nk∣xˇk,0\begin{array}{ll} \check{\boldsymbol{x}}_k\boldsymbol{f}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_k, \mathbf{0}\right) \check{\boldsymbol{y}}_k\boldsymbol{g}\left(\check{\boldsymbol{x}}_k, \mathbf{0}\right) \\ \boldsymbol{F}_{k-1}\left.\frac{\partial \boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_k, \boldsymbol{w}_k\right)}{\partial \boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_k, \mathbf{0}} \boldsymbol{G}_k\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right)}{\partial \boldsymbol{x}_k}\right|_{\check{\boldsymbol{x}}_k, \mathbf{0}} \\ \boldsymbol{B}_{k-1}\left.\frac{\partial \boldsymbol{f}\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_k, \boldsymbol{w}_k\right)}{\partial \boldsymbol{w}_k}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_k, \mathbf{0}} \boldsymbol{C}_k\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right)}{\partial \boldsymbol{n}_k}\right|_{\check{\boldsymbol{x}}_k, \mathbf{0}} \end{array} xˇkf(x^k−1,vk,0)Fk−1∂xk−1∂f(xk−1,vk,wk)x^k−1,vk,0Bk−1∂wk∂f(xk−1,vk,wk)x^k−1,vk,0yˇkg(xˇk,0)Gk∂xk∂g(xk,nk)xˇk,0Ck∂nk∂g(xk,nk)xˇk,0根据该线性化展开结果可以得到预测状态的统计学特征为 E[xk]≈xˇkFk−1(xk−1−x^k−1)E[Bk−1wk]⏟0E[(xk−E[xk])(xk−E[xk])T]≈E[Bk−1wkwTTBk−1T]⏟Bk−1QkBk−1T\begin{aligned} E\left[\boldsymbol{x}_k\right] \approx \check{\boldsymbol{x}}_k\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)\underbrace{E\left[\boldsymbol{B}_{k-1} \boldsymbol{w}_k\right]}_0 \\ E\left[\left(\boldsymbol{x}_k-E\left[\boldsymbol{x}_k\right]\right)\left(\boldsymbol{x}_k-E\left[\boldsymbol{x}_k\right]\right)^{\mathrm{T}}\right] \approx \underbrace{E\left[\boldsymbol{B}_{k-1} \boldsymbol{w}_k \boldsymbol{w}_{\mathrm{T}}^{\mathrm{T}} \boldsymbol{B}_{k-1}^{\mathrm{T}}\right]}_{\boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^{\mathrm{T}}} \end{aligned} E[xk]≈xˇkFk−1(xk−1−x^k−1)0E[Bk−1wk]E[(xk−E[xk])(xk−E[xk])T]≈Bk−1QkBk−1TE[Bk−1wkwTTBk−1T]即 p(xk∣xk−1,vk)≈N(xˇkFk−1(xk−1−x^k−1),Bk−1QkBk−1T)p\left(\boldsymbol{x}_k \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_k\right) \approx \mathcal{N}\left(\check{\boldsymbol{x}}_k\boldsymbol{F}_{k-1}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right), \boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^{\mathrm{T}}\right)p(xk∣xk−1,vk)≈N(xˇkFk−1(xk−1−x^k−1),Bk−1QkBk−1T) 同理可得到观测的统计学特征为 E[yk]≈yˇkGk(xk−xˇk)E[Cknk]⏟0E[(yk−E[yk])(yk−E[yk])T]≈E[CknknkTCkT]⏟CkRkCkT\begin{aligned} E\left[\boldsymbol{y}_k\right] \approx \check{\boldsymbol{y}}_k\boldsymbol{G}_k\left(\boldsymbol{x}_k-\check{\boldsymbol{x}}_k\right)\underbrace{E\left[\boldsymbol{C}_k \boldsymbol{n}_k\right]}_0 \\ E\left[\left(\boldsymbol{y}_k-E\left[\boldsymbol{y}_k\right]\right)\left(\boldsymbol{y}_k-E\left[\boldsymbol{y}_k\right]\right)^{\mathrm{T}}\right] \approx \underbrace{E\left[\boldsymbol{C}_k \boldsymbol{n}_k \boldsymbol{n}_k^{\mathrm{T}} \boldsymbol{C}_k^{\mathrm{T}}\right]}_{C_k \boldsymbol{R}_k \boldsymbol{C}_k^{\mathrm{T}}} \end{aligned} E[yk]≈yˇkGk(xk−xˇk)0E[Cknk]E[(yk−E[yk])(yk−E[yk])T]≈CkRkCkTE[CknknkTCkT]即 p(yk∣xk)≈N(yˇkGk(xk−xˇk),CkRkCkT)p\left(\boldsymbol{y}_k \mid \boldsymbol{x}_k\right) \approx \mathcal{N}\left(\check{\boldsymbol{y}}_k\boldsymbol{G}_k\left(\boldsymbol{x}_k-\check{\boldsymbol{x}}_k\right), \boldsymbol{C}_k \boldsymbol{R}_k \boldsymbol{C}_k^{\mathrm{T}}\right)p(yk∣xk)≈N(yˇkGk(xk−xˇk),CkRkCkT)
把均值和方差的具体形式带入广义高斯滤波的公式最终得到 EKF 下得经典五个公式。 PˇkFk−1P^k−1Fk−1TBk−1QkBk−1Txˇkf(x^k−1,vk,0)KkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(I−KkGk)Pˇkx^kxˇkKk(yk−g(xˇk,0))\begin{aligned} \check{\boldsymbol{P}}_k\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}\boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^{\mathrm{T}} \\ \check{\boldsymbol{x}}_k\boldsymbol{f}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_k, \mathbf{0}\right) \\ \boldsymbol{K}_k\check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\left(\boldsymbol{G}_k \check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\boldsymbol{C}_k \boldsymbol{R}_k \boldsymbol{C}_k^{\mathrm{T}}\right)^{-1} \\ \hat{\boldsymbol{P}}_k\left(\mathbf{I}-\boldsymbol{K}_k \boldsymbol{G}_k\right) \check{\boldsymbol{P}}_k \\ \hat{\boldsymbol{x}}_k\check{\boldsymbol{x}}_k\boldsymbol{K}_k\left(\boldsymbol{y}_k-\boldsymbol{g}\left(\check{\boldsymbol{x}}_k, \mathbf{0}\right)\right) \end{aligned} PˇkFk−1P^k−1Fk−1TBk−1QkBk−1Txˇkf(x^k−1,vk,0)KkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(I−KkGk)Pˇkx^kxˇkKk(yk−g(xˇk,0))
3.5 迭代扩展卡尔曼滤波(IEKF)推导
由于非线性模型中做了线性化近似当非线性程度越强时误差就会较大。但是由于线性化的工作点离真值越近线性化的误差就越小因此解决该问题的一个方法是通过迭代逐渐找到准确的线性化点从而提高精度。 在EKF的推导中其他保持不变仅改变观测的线性化工作点则有 g(xk,nk)≈yop,kGk(xk−xop,k)Cknk\boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right) \approx \boldsymbol{y}_{\mathrm{op}, k}\boldsymbol{G}_k\left(\boldsymbol{x}_k-\boldsymbol{x}_{\mathrm{op}, k}\right)\boldsymbol{C}_k \boldsymbol{n}_k g(xk,nk)≈yop,kGk(xk−xop,k)Cknk其中 yop,kg(xop,k,0)Gk∂g(xk,nk)∂xk∣xop,k,0Ck∂g(xk,nk)∂nk∣xop,k,0\begin{aligned} \boldsymbol{y}_{\mathrm{op}, k}\boldsymbol{g}\left(\boldsymbol{x}_{\mathrm{op}, k}, \mathbf{0}\right) \\ \boldsymbol{G}_k\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right)}{\partial \boldsymbol{x}_k}\right|_{\boldsymbol{x}_{\mathrm{op}, k}, \mathbf{0}} \\ \boldsymbol{C}_k\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_k, \boldsymbol{n}_k\right)}{\partial \boldsymbol{n}_k}\right|_{\boldsymbol{x}_{\mathrm{op}, \boldsymbol{k}}, \mathbf{0}} \end{aligned} yop,kg(xop,k,0)Gk∂xk∂g(xk,nk)xop,k,0Ck∂nk∂g(xk,nk)xop,k,0按照与之前同样的方式进行推导可得到滤波的校正过程为 KkPˇkGkT(GkPˇkGkTCkRkCkT)−1x^kxˇkKk(yk−yop,k−Gk(xˇk−xop,k))\begin{aligned} \boldsymbol{K}_k\check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\left(\boldsymbol{G}_k \check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\boldsymbol{C}_k \boldsymbol{R}_k \boldsymbol{C}_k^{\mathrm{T}}\right)^{-1} \\ \hat{\boldsymbol{x}}_k\check{\boldsymbol{x}}_k\boldsymbol{K}_k\left(\boldsymbol{y}_k-\boldsymbol{y}_{\mathrm{op}, k}-\boldsymbol{G}_k\left(\check{\boldsymbol{x}}_k-\boldsymbol{x}_{\mathrm{op}, k}\right)\right) \end{aligned} KkPˇkGkT(GkPˇkGkTCkRkCkT)−1x^kxˇkKk(yk−yop,k−Gk(xˇk−xop,k))可见唯一的区别是后验均值 x^k\hat{\boldsymbol{x}}_kx^k 更新的公式与之前有所不同。 滤波过程中反复执行这 2 个公式以上次的后验均值作为本次的线性化工作点即可达到减小非线性误差的目的。 需要注意的是在这种滤波模式下 后验方差应放在最后一步进行。 P^k(1−KkGk)Pˇk\hat{\boldsymbol{P}}_k\left(\mathbf{1}-\boldsymbol{K}_k \boldsymbol{G}_k\right) \check{\boldsymbol{P}}_k P^k(1−KkGk)Pˇk
4. 基于滤波器的融合
通过以上推导滤波问题可以简单理解为“预测 观测 融合结果”。 结合实际点云地图中定位的例子预测由IMU给出观测即为激光雷达点云和地图匹配得到的姿态和位置。 融合流程用框图可以表示如下
4.1 状态方程
状态方程 FFF 由误差方程得来第8讲已经完成误差方程的推导 δp˙δvδv˙−Rt[at−bat]×δθRt(na−δba)δθ˙−[ωt−bωt]×δθnω−δbωδb˙anba或 δb˙a0δb˙ωnbωδb˙ω0令 δx[δpδvδθδbaδbω],w[nanωnbanbω]\begin{aligned} \delta \dot{\boldsymbol{p}}\delta \boldsymbol{v} \\ \delta \dot{\boldsymbol{v}}-\boldsymbol{R}_t\left[\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right]_{\times} \delta \boldsymbol{\theta}\boldsymbol{R}_t\left(\boldsymbol{n}_a-\delta \boldsymbol{b}_a\right) \\ \delta \dot{\boldsymbol{\theta}}-\left[\boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t}\right]_{\times} \delta \boldsymbol{\theta}\boldsymbol{n}_\omega-\delta \boldsymbol{b}_\omega \\ \delta \dot{\boldsymbol{b}}_a\boldsymbol{n}_{b_a} \quad \text { 或 } \quad \delta \dot{\boldsymbol{b}}_a0 \\ \delta \dot{\boldsymbol{b}}_\omega\boldsymbol{n}_{b_\omega} \quad\quad\quad { }^{ }{ }^{ }{ }^{ }{ }^{ }{ }^{ }{ }^{ } \delta \dot{\boldsymbol{b}}_\omega0 \\ \text { 令 } \delta \boldsymbol{x}\left[\begin{array}{c} \delta \boldsymbol{p} \\ \delta \boldsymbol{v} \\ \delta \boldsymbol{\theta} \\ \delta \boldsymbol{b}_a \\ \delta \boldsymbol{b}_\omega \end{array}\right], \quad \boldsymbol{w}\left[\begin{array}{l} \boldsymbol{n}_a \\ \boldsymbol{n}_\omega \\ \boldsymbol{n}_{b_a} \\ \boldsymbol{n}_{b_\omega} \end{array}\right] \end{aligned} δp˙δvδv˙−Rt[at−bat]×δθRt(na−δba)δθ˙−[ωt−bωt]×δθnω−δbωδb˙anba 或 δb˙a0δb˙ωnbωδb˙ω0 令 δxδpδvδθδbaδbω,wnanωnbanbω则误差方程可以写成状态方程的通用形式 δx˙FtδxBtw\delta \dot{\boldsymbol{x}}\boldsymbol{F}_t \delta \boldsymbol{x}\boldsymbol{B}_t \boldsymbol{w} δx˙FtδxBtw 其中 Ft[0I300000−Rt[a‾t]×−Rt000−[ω‾t]×0−I30000000000]a‾tat−batω‾tωt−bωtBt[0000Rt0000I30000I30000I3]\begin{aligned} \boldsymbol{F}_t \left[\begin{array}{ccccc} 0 \boldsymbol{I}_3 \mathbf{0} \mathbf{0} \mathbf{0} \\ \mathbf{0} \mathbf{0} -\boldsymbol{R}_t\left[\overline{\boldsymbol{a}}_t\right]_{\times} -\boldsymbol{R}_t \mathbf{0} \\ 0 0 -\left[\overline{\boldsymbol{\omega}}_t\right]_{\times} \mathbf{0} -\boldsymbol{I}_3 \\ 0 0 0 0 0 \\ 0 0 0 0 0 \end{array}\right] \begin{array}{c} \overline{\boldsymbol{a}}_t\boldsymbol{a}_t-\boldsymbol{b}_{a_t} \\ \overline{\boldsymbol{\omega}}_t\boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t} \end{array} \\ \boldsymbol{B}_t \left[\begin{array}{cccc} \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \\ \boldsymbol{R}_t \mathbf{0} \mathbf{0} \mathbf{0} \\ \mathbf{0} \boldsymbol{I}_3 \mathbf{0} \mathbf{0} \\ \mathbf{0} \mathbf{0} \boldsymbol{I}_3 \mathbf{0} \\ \mathbf{0} \mathbf{0} \mathbf{0} \boldsymbol{I}_3 \end{array}\right] \end{aligned} FtBt00000I300000−Rt[at]×−[ωt]×000−Rt00000−I300atat−batωtωt−bωt0Rt00000I300000I300000I3注当选择 δb˙a0δb˙ω0\delta \dot{\boldsymbol{b}}_a0 \delta \dot{\boldsymbol{b}}_\omega0δb˙a0δb˙ω0 时矩阵形式不一样请各位自行推导。
4.2 观测方程
在滤波器中观测方程 GGG 一般写为 yGtδxCtn\boldsymbol{y}\boldsymbol{G}_t \delta \boldsymbol{x}\boldsymbol{C}_t \boldsymbol{n} yGtδxCtn此例中观测量有位置、失准角则 y[δp‾δθ‾]\boldsymbol{y}\left[\begin{array}{l} \delta \overline{\boldsymbol{p}} \\ \delta \overline{\boldsymbol{\theta}} \end{array}\right] y[δpδθ]因此有 Gt[I3000000I300]Ct[I300I3]\begin{aligned} \boldsymbol{G}_t \left[\begin{array}{ccccc} \boldsymbol{I}_3 \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \\ \mathbf{0} \mathbf{0} \boldsymbol{I}_3 \mathbf{0} \mathbf{0} \end{array}\right] \\ \boldsymbol{C}_t \left[\begin{array}{cc} \boldsymbol{I}_3 \mathbf{0} \\ \mathbf{0} \boldsymbol{I}_3 \end{array}\right] \end{aligned} GtCt[I30000I30000][I300I3]而此处 nnn 为观测噪声 n[nδpˉxnδpˉynδpˉznδθˉxnδθˉynδθˉz]T\boldsymbol{n}\left[\begin{array}{llllll} n_{\delta \bar{p}_x} n_{\delta \bar{p}_y} n_{\delta \bar{p}_z} n_{\delta \bar{\theta}_x} n_{\delta \bar{\theta}_y} n_{\delta \bar{\theta}_z} \end{array}\right]^T n[nδpˉxnδpˉynδpˉznδθˉxnδθˉynδθˉz]T观测量中 δp\delta \boldsymbol{p}δp 的计算过程为 δp‾pˇ−p\delta \overline{\boldsymbol{p}}\check{\boldsymbol{p}}-\boldsymbol{p} δppˇ−p其中 pˇ\check{\boldsymbol{p}}pˇ 为 IMU 解算的位置即预测值。 p\boldsymbol{p}p 为雷达与地图 匹配得到的位置即观测值。 δθ‾\delta \overline{\boldsymbol{\theta}}δθ 的计算过程稍微复杂需要先计算误差矩阵 δR‾tRtTRˇt\delta \overline{\boldsymbol{R}}_t\boldsymbol{R}_t^T \check{\boldsymbol{R}}_t δRtRtTRˇt其中 Rˇt\check{\boldsymbol{R}}_tRˇt 为 IMU 解算的旋转矩阵即预测值。Rt\boldsymbol{R}_tRt 为雷达与地图匹配得到的旋转矩阵即观测值。 由于预测值与观测值之间的关系为 Rˇt≈Rt(I[δθ‾]×)\check{\boldsymbol{R}}_t \approx \boldsymbol{R}_t\left(\boldsymbol{I}[\delta \overline{\boldsymbol{\theta}}]_{\times}\right) Rˇt≈Rt(I[δθ]×)因此 δθ‾(δR‾t−I)∨\delta \overline{\boldsymbol{\theta}}\left(\delta \overline{\boldsymbol{R}}_t-\boldsymbol{I}\right)^{\vee} δθ(δRt−I)∨
4.3 构建滤波器
构建滤波器即把融合系统的状态方程和观测方程应用到 Kalman 滤波的五个公式中。 前面推导的方程是连续时间的要应用于离散时间需要按照采样时间对其进行离散化。 状态方程离散化可以写为 δxkFk−1δxk−1Bk−1wk\delta \boldsymbol{x}_k\boldsymbol{F}_{k-1} \delta \boldsymbol{x}_{k-1}\boldsymbol{B}_{k-1} \boldsymbol{w}_k δxkFk−1δxk−1Bk−1wk其中 Fk−1I15FtTBk−1[0000Rk−1T0000I3T0000I3T0000I3T]\begin{aligned} \boldsymbol{F}_{k-1} \boldsymbol{I}_{15}\boldsymbol{F}_t T \\ \boldsymbol{B}_{k-1} {\left[\begin{array}{cccc} \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \\ \boldsymbol{R}_{k-1} T \mathbf{0} \mathbf{0} \mathbf{0} \\ \mathbf{0} \boldsymbol{I}_3 T \mathbf{0} \mathbf{0} \\ \mathbf{0} \mathbf{0} \boldsymbol{I}_3 \sqrt{T} \mathbf{0} \\ \mathbf{0} \mathbf{0} \mathbf{0} \boldsymbol{I}_3 \sqrt{T} \end{array}\right] } \end{aligned} Fk−1Bk−1I15FtT0Rk−1T00000I3T00000I3T00000I3T其中TTT 为 Kalman 的滤波周期。 注关于 Bk−1\boldsymbol{B}_{k-1}Bk−1 的离散化形式不同资料有差异但对实际调试影响不大。 对于观测方程不需要乘以滤波周期可以直接写出 ykGkδxkCknk\boldsymbol{y}_k\boldsymbol{G}_k \delta \boldsymbol{x}_k\boldsymbol{C}_k \boldsymbol{n}_k ykGkδxkCknk将以上各变量带入kalman滤波的五个方程即可构建完整的滤波器更新流程。 δxˇkFk−1δx^k−1Bk−1wkPˇkFk−1P^k−1Fk−1TBk−1QkBk−1TKkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(I−KkGk)Pˇkδx^kδxˇkKk(yk−Gkδxˇk)\begin{aligned} \delta \check{\boldsymbol{x}}_k\boldsymbol{F}_{k-1} \delta \hat{\boldsymbol{x}}_{k-1}\boldsymbol{B}_{k-1} \boldsymbol{w}_k \\ \check{\boldsymbol{P}}_k\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}\boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^T \\ \boldsymbol{K}_k\check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\left(\boldsymbol{G}_k \check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\boldsymbol{C}_k \boldsymbol{R}_k \boldsymbol{C}_k^T\right)^{-1} \\ \hat{\boldsymbol{P}}_k\left(\boldsymbol{I}-\boldsymbol{K}_k \boldsymbol{G}_k\right) \check{\boldsymbol{P}}_k \\ \delta \hat{\boldsymbol{x}}_k\delta \check{\boldsymbol{x}}_k\boldsymbol{K}_k\left(\boldsymbol{y}_k-\boldsymbol{G}_k \delta \check{\boldsymbol{x}}_k\right) \end{aligned} δxˇkFk−1δx^k−1Bk−1wkPˇkFk−1P^k−1Fk−1TBk−1QkBk−1TKkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(I−KkGk)Pˇkδx^kδxˇkKk(yk−Gkδxˇk)
4.4 Kalman 滤波实际使用流程
4.4.1 位姿初始化 在点云地图中实现初始定位并给以下变量赋值 p^0\hat{\boldsymbol{p}}_0p^0 初始时刻位置 v^0\hat{\boldsymbol{v}}_0v^0 初始时刻速度(可以从组合导航获得) R^0\hat{\boldsymbol{R}}_0R^0 : 初始时刻姿态(也可用四元数后面不再强调)
4.4.2 Kalman 初始化
a. 状态量 δx^00\delta \hat{\boldsymbol{x}}_0\mathbf{0}δx^00 b. 方差 P^0[PδpPδvPδθPδbaPδbω]\hat{\boldsymbol{P}}_0\left[\begin{array}{ccccc} \boldsymbol{P}_{\delta p} \\ \boldsymbol{P}_{\delta v} \\ \boldsymbol{P}_{\delta \boldsymbol{\theta}} \\ \boldsymbol{P}_{\delta b_a} \\ \boldsymbol{P}_{\delta b_\omega} \end{array}\right] P^0PδpPδvPδθPδbaPδbω初始方差理论上可设置为各变量噪声的平方实际中一般故意设置大一些这样可加快收敛速度。 c. 过程噪声与观测噪声 Q[QaQωQbaQbω]R0[RδpRδθ]\boldsymbol{Q}\left[\begin{array}{cccc} \boldsymbol{Q}_a \\ \boldsymbol{Q}_\omega \\ \boldsymbol{Q}_{b_a} \\ \boldsymbol{Q}_{b_\omega} \end{array}\right] \quad \quad \boldsymbol{R}_0\left[\begin{array}{ll} \boldsymbol{R}_{\delta p} \\ \boldsymbol{R}_{\delta \theta} \end{array}\right] QQaQωQbaQbωR0[RδpRδθ]过程噪声与观测噪声一般在 kalman 迭代过程中保持不变。
4.4.3 惯性解算 按照之前讲解的惯性解算方法进行位姿更新该位姿属于先验位姿。 a. 姿态解算 RˇkR^k−1(Isinϕϕ(ϕ×)1−cosϕϕ2(ϕ×)2)\check{\boldsymbol{R}}_k\hat{\boldsymbol{R}}_{k-1}\left(I\frac{\sin \phi}{\phi}(\phi \times)\frac{1-\cos \phi}{\phi^2}(\boldsymbol{\phi} \times)^2\right) RˇkR^k−1(Iϕsinϕ(ϕ×)ϕ21−cosϕ(ϕ×)2)其中 ϕω‾k−1ω‾k2(tk−tk−1)ω‾kωk−bωkω‾k−1ωk−1−bωk−1\begin{aligned} \boldsymbol{\phi}\frac{\overline{\boldsymbol{\omega}}_{k-1}\overline{\boldsymbol{\omega}}_k}{2}\left(t_k-t_{k-1}\right) \\ \overline{\boldsymbol{\omega}}_k\boldsymbol{\omega}_k-\boldsymbol{b}_{\omega_k} \\ \overline{\boldsymbol{\omega}}_{k-1}\boldsymbol{\omega}_{k-1}-\boldsymbol{b}_{\omega_{k-1}} \end{aligned} ϕ2ωk−1ωk(tk−tk−1)ωkωk−bωkωk−1ωk−1−bωk−1按照之前讲解的惯性解算方法进行位姿更新该位姿属于先验位姿。 b. 速度解算 vˇkv^k−1(Rˇka‾kR^k−1a‾k−12−g)(tk−tk−1)\check{\boldsymbol{v}}_k\hat{\boldsymbol{v}}_{k-1}\left(\frac{\check{\boldsymbol{R}}_k \overline{\boldsymbol{a}}_k\hat{\boldsymbol{R}}_{k-1} \overline{\boldsymbol{a}}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_k-t_{k-1}\right) vˇkv^k−1(2RˇkakR^k−1ak−1−g)(tk−tk−1) 其中 a‾kak−baka‾k−1ak−1−bak−1\begin{aligned} \overline{\boldsymbol{a}}_k\boldsymbol{a}_k-\boldsymbol{b}_{a_k} \\ \overline{\boldsymbol{a}}_{k-1}\boldsymbol{a}_{k-1}-\boldsymbol{b}_{a_{k-1}} \end{aligned} akak−bakak−1ak−1−bak−1c. 位置解算 p^kpˇk−1vˇkv^k−12(tk−tk−1)\hat{\boldsymbol{p}}_k\check{\boldsymbol{p}}_{k-1}\frac{\check{\boldsymbol{v}}_k\hat{\boldsymbol{v}}_{k-1}}{2}\left(t_k-t_{k-1}\right) p^kpˇk−12vˇkv^k−1(tk−tk−1)
4.4.4 Kalman 预测更新 执行kalman五个步骤中的前两步即 δxˇkFk−1δx^k−1Bk−1wkPˇkFk−1P^k−1Fk−1TBk−1QkBk−1T\begin{aligned} \delta \check{\boldsymbol{x}}_k\boldsymbol{F}_{k-1} \delta \hat{\boldsymbol{x}}_{k-1}\boldsymbol{B}_{k-1} \boldsymbol{w}_k \\ \check{\boldsymbol{P}}_k\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}\boldsymbol{B}_{k-1} \boldsymbol{Q}_k \boldsymbol{B}_{k-1}^T \end{aligned} δxˇkFk−1δx^k−1Bk−1wkPˇkFk−1P^k−1Fk−1TBk−1QkBk−1T当然这需要先根据公式计算 Fk−1\boldsymbol{F}_{k-1}Fk−1 和 Bk−1\boldsymbol{B}_{k-1}Bk−1 。
4.4.5 无观测时后验更新 无观测时不需要执行kalman剩下的三个步骤后验等于先验即 δx^kδxˇkP^kPˇkx^kxˇk\begin{aligned} \delta \hat{\boldsymbol{x}}_k\delta \check{\boldsymbol{x}}_k \\ \hat{\boldsymbol{P}}_k\check{\boldsymbol{P}}_k \\ \hat{\boldsymbol{x}}_k\check{\boldsymbol{x}}_k \end{aligned} δx^kδxˇkP^kPˇkx^kxˇk
4.4.6 有观测时的量测更新 执行kalman滤波后面的三个步骤得到后验状态量。 KkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(I−KkGk)Pˇkδx^kδxˇkKk(yk−Gkδxˇk)\begin{aligned} \boldsymbol{K}_k\check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\left(\boldsymbol{G}_k \check{\boldsymbol{P}}_k \boldsymbol{G}_k^{\mathrm{T}}\boldsymbol{C}_k \boldsymbol{R}_k \boldsymbol{C}_k^T\right)^{-1} \\ \hat{\boldsymbol{P}}_k\left(\boldsymbol{I}-\boldsymbol{K}_k \boldsymbol{G}_k\right) \check{\boldsymbol{P}}_k \\ \delta \hat{\boldsymbol{x}}_k\delta \check{\boldsymbol{x}}_k\boldsymbol{K}_k\left(\boldsymbol{y}_k-\boldsymbol{G}_k \delta \check{\boldsymbol{x}}_k\right) \end{aligned} KkPˇkGkT(GkPˇkGkTCkRkCkT)−1P^k(I−KkGk)Pˇkδx^kδxˇkKk(yk−Gkδxˇk)
4.4.7 有观测时计算后验位姿 根据后验状态量更新后验位姿。 p^kpˇk−δp^kv^kvˇk−δv^kR^kRˇk(I−[δθ^k]×)b^akbˇak−δb^akb^ωkbˇωk−δb^ωk\begin{aligned} \hat{\boldsymbol{p}}_k\check{\boldsymbol{p}}_k-\delta \hat{\boldsymbol{p}}_k \\ \hat{\boldsymbol{v}}_k\check{\boldsymbol{v}}_k-\delta \hat{\boldsymbol{v}}_k \\ \hat{\boldsymbol{R}}_k\check{\boldsymbol{R}}_k\left(\boldsymbol{I}-\left[\delta \hat{\boldsymbol{\theta}}_k\right]_{\times}\right) \\ \hat{\boldsymbol{b}}_{a_k}\check{\boldsymbol{b}}_{a_k}-\delta \hat{\boldsymbol{b}}_{a_k} \\ \hat{\boldsymbol{b}}_{\omega_k}\check{\boldsymbol{b}}_{\omega_k}-\delta \hat{\boldsymbol{b}}_{\omega_k} \end{aligned} p^kpˇk−δp^kv^kvˇk−δv^kR^kRˇk(I−[δθ^k]×)b^akbˇak−δb^akb^ωkbˇωk−δb^ωk
4.4.8 有观测时状态量清零 状态量已经用来补偿因此需要清零。 δx^k0\delta \hat{\boldsymbol{x}}_k\mathbf{0} δx^k0后验方差保持不变。
4.4.9 输出位姿
把后验位姿输出给其他模块使用即输出 p^k\hat{\boldsymbol{p}}_kp^kv^k\hat{\boldsymbol{v}}_kv^kR^k\hat{\boldsymbol{R}}_kR^k (或 q^k\hat{\boldsymbol{q}}_kq^k)。
