迭代扩展卡尔曼滤波IEKF

预测

迭代扩展卡尔曼滤波（IEKF）的预测部分和扩展卡尔曼滤波基本相同。直接给出结论：

xˇk=f(x^k−1,vk,0)\check{\boldsymbol{x}}_{k} =\boldsymbol{f}\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \mathbf{0}\right) xˇk=f(x^k−1,vk,0)

Pˇk=Fk−1P^k−1Fk−1T+Qk′\check{\boldsymbol{P}}_{k} =\boldsymbol{F}_{k-1} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}_{k-1}^{\mathrm{T}}+\boldsymbol{Q}_{k}^{\prime} Pˇk=Fk−1P^k−1Fk−1T+Qk′

更新

非线性观测模型为：

yk=g(xk,nk)\boldsymbol{y}_{k}=g\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right) yk=g(xk,nk)

对其中任意一个点 xop,k\boldsymbol{x}_{\mathrm{op}, k}xop,k 进行线性化，可得：

g(xk,nk)≈yop,k+Gk(xk−xop,k)+nk′\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{n}_{k}^{\prime} g(xk,nk)≈yop,k+Gk(xk−xop,k)+nk′

其中：

yop,k=g(xop,k,0)\boldsymbol{y}_{\mathrm{op}, k}=\boldsymbol{g}\left(\boldsymbol{x}_{\mathrm{op}, k}, \mathbf{0}\right)yop,k=g(xop,k,0)
Gk=∂g(xk,nk)∂xk∣xop,k,0\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}}Gk=∂xk∂g(xk,nk)∣∣xop,k,0
nk′=∂g(xk,nk)∂nk∣xop,k,0nk\boldsymbol{n}_{k}^{\prime}=\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right)}{\partial \boldsymbol{n}_{k}}\right|_{\boldsymbol{x}_{\mathrm{op}, k}, \mathbf{0}} \boldsymbol{n}_{k}nk′=∂nk∂g(xk,nk)∣∣xop,k,0nk

任意一个点 xop,k\boldsymbol{x}_{\mathrm{op}, k}xop,k 进行线性化，可得：

需要注意的是，观测模型和雅可比矩阵均在 xop,k\boldsymbol{x}_{\mathrm{op}, k}xop,k 处计算。（在EKF中， xop,k=xˇk\boldsymbol{x}_{\mathrm{op}, k}=\check{\boldsymbol{x}}_{k}xop,k=xˇk）

使用上面的线性化模型，我们可以将时刻 kkk 处的状态和测量的联合概率近似为高斯分布，即：

p(xk,yk∣xˇ0,v1:k,y0:k−1)≈N([μx,kμy,k],[Σxx,kΣxy,kΣyx,kΣyy,k])=N([xˇkyop,k+Gk(xˇk−xop,k)],[PˇkPˇkGkTGkPˇkGkPˇkGkT+Rk′])\begin{aligned}& p\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \approx \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) \\=& \mathcal{N}\left(\left[\begin{array}{cc}\check{\boldsymbol{x}}_{k} \\\boldsymbol{y}_{\mathrm{op}, k}+\boldsymbol{G}_{k}\left(\check{\boldsymbol{x}}_{k}-\boldsymbol{x}_{\mathrm{op}, k}\right)\end{array}\right],\left[\begin{array}{cc}\check{\boldsymbol{P}}_{k} & \check{\boldsymbol{P}}_{k}\boldsymbol{G}_{k}^{\mathrm{T}} \\\boldsymbol{G}_{k} \check{\boldsymbol{P}}_{k} & \boldsymbol{G}_{k} \check{\boldsymbol{P}}_{k} \boldsymbol{G}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}^{\prime}\end{array}\right]\right)\end{aligned} =p(xk,yk∣xˇ0,v1:k,y0:k−1)≈N([μx,kμy,k],[Σxx,kΣyx,kΣxy,kΣyy,k])N([xˇkyop,k+Gk(xˇk−xop,k)],[PˇkGkPˇkPˇkGkTGkPˇkGkT+Rk′])

如果测量值 yk\boldsymbol{y}_{k}yk 已知，我们可以利用高斯推断得到 xk\boldsymbol{x}_{k}xk 的条件概率密度（即后验）：

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{\boldsymbol{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)

令 Kk=Σxy,kΣyy,k−1=PˇkGkT(GkPˇkGkT+Rk′)−1\boldsymbol{K}_{k}=\boldsymbol{\Sigma}_{x y,k} \boldsymbol{\Sigma}_{y y,k}^{-1}=\check{\boldsymbol{P}}_{k} \boldsymbol{G}_{k}^{\mathrm{T}}\left(\boldsymbol{G}_{k} \check{\boldsymbol{P}}_{k} \boldsymbol{G}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}^{\prime}\right)^{-1}Kk=Σxy,kΣyy,k−1=PˇkGkT(GkPˇkGkT+Rk′)−1（卡尔曼增益），则得到：

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

x^k=xˇk+Kk(yk−yop,k−Gk(xˇk−xop,k))\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) x^k=xˇk+Kk(yk−yop,k−Gk(xˇk−xop,k))

可以看出，IEKF中的卡尔曼增益和更新方程与EKF非常相似，唯一的区别在于线性化的工作点。如果将线性化的工作点设置为预测先验的均值（即 xop,k=xˇk\boldsymbol{x}_{\mathrm{op}, k}=\check{\boldsymbol{x}}_{k}xop,k=xˇk），那么IEKF就等价于EKF。

然而，如果我们迭代的重新计算 x^k\hat{\boldsymbol{x}}_{k}x^k ，并且在每一次迭代中将工作点设置为上一次迭代的后验均值（即令 xop,k=x^k\boldsymbol{x}_{\mathrm{op}, k}=\hat{\boldsymbol{x}}_{k}xop,k=x^k ），将得到更好的结果。

在第一次迭代中, 我们令 xop,k=xˇk\boldsymbol{x}_{\mathrm{op}, k}=\check{\boldsymbol{x}}_{k}xop,k=xˇk，这使得我们能够对更好的估计进行线性化, 从而改进每次迭代的近似程度。在迭代的过程中，若 xop,k\boldsymbol{x}_{\mathrm{op}, k}xop,k 的改变足够小就终止迭代。

注意，卡尔曼增益方程和更新方程收敛之后，协方差方程只需要计算一次。

迭代扩展卡尔曼滤波IEKF相关推荐

【目标跟踪】基于迭代扩展卡尔曼滤波算法实现目标滤波跟踪（IEKF）附Matlab代码
1 简介 Object tracking is a well studied fifield in computer vision. The goal is to use a sensor sucha ...
迭代扩展卡尔曼滤波学习
论文名字<The iterated Kalman filter update as a Gauss-Newton method> 引言: 计算资源大,IKF比EKF效果更好.IKF的更新方 ...
基于扩展卡尔曼滤波的SOC估计（附MATLAB代码）
1.卡尔曼滤波原理原理可以参考我之前学习的笔记,使用goodnote完成的. 我认为,对于公式的推导不需要做太多深入的了解,我之前也对公式进行推导的理解,但是没过几天就忘了,只需要掌握住那重要的5个 ...
基于扩展卡尔曼滤波(EKF)的机器人状态估计
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统,EK ...
卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）相应推导
从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献.为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波.扩展卡尔曼滤波.无迹卡尔曼滤波.粒子滤波等等方法,这些方法在 ...
【camera-lidar-radar】基于卡尔曼滤波和扩展卡尔曼滤波的相机、激光雷达、毫米波雷达多传感器后融合
[camera-lidar-radar]基于卡尔曼滤波和扩展卡尔曼滤波的相机.激光雷达.毫米波雷达多传感器后融合代码下载地址(C++ and Python):下载地址红点和蓝点分别表示radar和 ...
扩展卡尔曼滤波EKF与多传感器融合
Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本.在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准.本文将简要介绍EKF,并介绍其在无人驾 ...
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...
机器人学习--扩展卡尔曼滤波算法用于机器人定位
论文链接:leonard_ieeetroa19.pdf-互联网文档类资源-CSDN下载https://download.csdn.net/download/GGY1102/61742571 基于EKF ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...

迭代扩展卡尔曼滤波IEKF

迭代扩展卡尔曼滤波IEKF

预测

更新

迭代扩展卡尔曼滤波IEKF相关推荐

最新文章

热门文章