最小二乘图加入
这里有很多背景，请滚动到底部以查找问题 我正在尝试“ 线性最小二乘问题离SLAM有多远”中描述的地图连接算法；具体地，公式（36）。我写的代码似乎总是采用第二张地图的值作为地标位置。我的问题是，我是正确理解文本还是在犯某种错误。我将在理解公式的同时尝试说明它们，并说明我的代码如何实现这些公式。我正在尝试仅合并两个本地地图的简单情况。 从论文（36）中可以看出，将两个局部地图是在寻找一个状态向量，该状态向量将最小化：Xjoin,relXjoin,relX_{join,rel} ∑j=1k(XLj^−Hj,rel(Xjoin,rel))T(PLj)−1(XLj^−Hj,rel(Xjoin,rel))∑j=1k(XjL^−Hj,rel(Xjoin,rel))T(PjL)−1(XjL^−Hj,rel(Xjoin,rel)) \sum_{j=1}^{k}(\hat{X_j^L} - H_{j,rel}(X_{join,rel}))^T(P_j^L)^{-1}(\hat{X_j^L} - H_{j,rel}(X_{join,rel})) 扩展为两个本地地图和我有：XL1^X1L^\hat{X_1^L}XL2^X2L^\hat{X_2^L} (XL1^−Hj,rel(Xjoin,rel))T(PL1)−1(XL1^−Hj,rel(Xjoin,rel))+(XL2^−Hj,rel(Xjoin,rel))T(PL2)−1(XL2^−Hj,rel(Xjoin,rel))(X1L^−Hj,rel(Xjoin,rel))T(P1L)−1(X1L^−Hj,rel(Xjoin,rel))+(X2L^−Hj,rel(Xjoin,rel))T(P2L)−1(X2L^−Hj,rel(Xjoin,rel)) (\hat{X_1^L} - H_{j,rel}(X_{join,rel}))^T(P_1^L)^{-1}(\hat{X_1^L} - H_{j,rel}(X_{join,rel})) + (\hat{X_2^L} - H_{j,rel}(X_{join,rel}))^T(P_2^L)^{-1}(\hat{X_2^L} - H_{j,rel}(X_{join,rel})) 据我了解，子图可以看作是全局图的综合观察，因此是与子图相关联的噪声（而不是我用来制作子图的EKF中的过程噪声。可能不会有所不同）。PLjPjLP^L_j 向量是第一个地图的姿态，第二个地图的姿态以及两个地图中地标的并集。Xjoin,relXjoin,relX_{join,rel} 函数为：Hj,relHj,relH_{j,rel} ⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢Xr(j−1)erjeϕr(j−1)erjeR(ϕrmj1er(j−1)e)(Xrmj1efj1−Xrmj1er(j−1)e)...R(ϕrmjler(j−1)e)(Xrmjlefjl−Xrmjler(j−1)e)Xrj−1efj(l+1)...Xrj−1efjn⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥[Xrjer(j−1)eϕrjer(j−1)eR(ϕr(j−1)ermj1e)(Xfj1rmj1e−Xr(j−1)ermj1e)...R(ϕr(j−1)ermjle)(Xfjlrmjle−Xr(j−1)ermjle)Xfj(l+1)rj−1e...Xfjnrj−1e] \begin{bmatrix} X_{r_{je}}^{r_{(j-1)e}}\\ \phi_{r_{je}}^{r_{(j-1)e}}\\ R(\phi_{r_{(j-1)e}}^{r_{m_{j1}e}}) (X^{r_{m_{j1}e}}_{f_{j1}} - X^{r_{m_{j1}e}}_{r_{(j-1)e}})\\.\\.\\.\\ R(\phi_{r_{(j-1)e}}^{r_{m_{jl}e}}) (X^{r_{m_{jl}e}}_{f_{jl}} - X^{r_{m_{jl}e}}_{r_{(j-1)e}})\\ X_{f_{j(l+1)}}^{r_{j-1e}}\\ .\\.\\.\\ X_{f_{jn}}^{r_{j-1e}} \end{bmatrix} 我不相信我的以下评估是正确的： 前两个元素是机器人在上一张地图的参考框中的姿势。例如，对于地图1，该姿势将位于初始帧中；对于地图2，它将位于地图1的框架中。t0t0t_0 下一组元素是地图1和地图2共有的元素，它们被转换为地图1的参考系。 最后一行是地图2特有的功能，位于第一张地图的框架中。 我的matlab实现如下： function [G, fval, …