【基于因子图的增量平滑优化算法分析案例3200字】

1 11.2基于Givens旋转的增量平滑算法 1 3 3 5 71.1引言车达到实时解算和估计导航状态的目的，本章节介绍了两种基于增量平滑优化 (IncrementalSmoothingandMapping)方法，一种是基于Givens变换的增量平滑算法，该方法通过在QR分解的基础上通过Givens旋转对一个非线性目标函数进行线性化可以得到式3-47中的最小二乘问题，可以将它重新写成如式4-1所示：c代表残差平方和的常量，当一个新的测量值加进来时，可以得到式中aT为新加入的测量值，β为与之对应的b中的值，使用下述进行QR更新，的到一个新的矩阵如式4-3针对上式使用Givens旋转，对左侧新加入行进行消元，将待求矩阵中对角线左下的所有量乘以一个Givens旋转矩阵零测量值开始，选择参数φ使矩阵中第(i,k)个元素变成零。如图4-1所示。从最左边的非零项(i,k₁)开始转换，生成一个标准的上三角矩阵，其中R包含了更新因子。需要注意的是，进行Givens旋转变换后会在右上角新的行中引入其基于上述方法得到的求解过程在于不对加入新变量的A矩c为更新后的残差平方和，通过求解Rδ=d就可得到当前的状态变量，对于基于Givens旋转的方法来说，新的测量值加入后，使得之前的信息矩阵的一部分使得变量能够被重新排序和重新线性化，并且该算法在QR分解过程中会引入更1.3.1由因子图向贝叶斯网转换般选取COLAMDiil来获得接近最优的排序方式，确定消元顺序后，假设第一个要进行消元的变量为v,先找到所有与变量v;相关的因子节点，设为f,(v,s),s;代表f中的其他变量，同时将与f相连的变量之间除去v,这个变量的变量集和定义为S,即如式4-7所示：P(v.|S)将添加到到贝叶斯网中。003.将步骤2中的公式分解，得到关于一个新因子和概率密度函数的乘积如式对于图3-5来说我们对图中的x₁,c₁,x₂,C₂进行变量消元我们将其分别表示成如图其中f₁,f₂代表了图中的先验因子，f,f₄代表了IMU因子和零偏因子，f₅代表先消除变量x₁,与x₁相关的因子有f₁(x₁)、f₃(x,x₂,C₁),分离变量集S为f(x,S)=f(x)fs(x,x₂,c)="“(x₂,c)P(3)消除变量C₁4)消除变量x₂第一步消除x₁如图4-3中(2)所示，红色保留在了因子图中。同理按照顺序消除c₁得到消除x₂得到消除c₂得到P(x₂Ic₂)、P(c₂),这四个概率密度相当于贝叶斯网络节点，这样形成的贝叶斯网络如下图4-4同的，则可以知道对于生成的贝叶斯网络也是和QR分解得到的矩阵与因子图变量消元的顺序是相反的，设转换过程中首先需要转换的变量是v;,如果分离变量集S为空集，则创建一个新的团，并将变量v;插入该团中，该团也被与变量v,对应的节点所关联的所有变量集合Vn={v,S}相比较。如果Vm=S,则入新建的这个子团中，此时母团和子团之间也会存在变量的交集VmnVn交集作为子团的分离变量。具体步骤大致如表4-2所示：按照C₂,x₂,C₁,x₁的顺序进行转换可以得到如下图所示的贝叶斯树的结构，对应图按照与变量消元相反的顺序，对于第一个变量c₂来说对应式4-12,其中分离变量S为空集，则创建一个根团，将c₂加入到根团中如图4-6(1),此根团也相当对于第二个变量x₂来说，对应式4-11,分离变量S={c₂},关联节点中的变量集合为Vm={x₂,C₂},母团中的变量此时为Vm={c₂},可知此时Vm=S,直接将对于变量c₁来说，对应式4-10,分离变量S={C₂,x₂},关联节点的变量集合为Vn={x₂,C₂,C₁},母团中的变量为Vm={c₂,x₂},由Vm=S可知，直接将c₁加入母对于变量x₁,对应式4-9,分离变量为S={c₁,x₂},(1)转换C₂(3)转换C₁(2)转换X₂(4)转换x在图中可以发现对于新加入的变量和节点只影响了变量x₂,C₂,我们按照用4-13结果更新贝叶斯网络，可以得到如图4-8(a)所示的前后对比图，如图4-8中红色虚线和员卷所示，可以看出对于加入的变量x,c₃只影响了变量节点x₂,C₂,并未对x,c₁产生影响，对于更新的贝叶斯树可以