小编Gou*_*uda的帖子

我试图通过使用我的网络摄像头从具有已知全球位置的四个基准点的图像中获得全局姿势估计.

我检查了很多stackexchange问​​题和一些文章,我似乎无法得到一个正确的解决方案.我得到的位置编号是可重复的,但绝不与摄像机移动成线性比例.仅供参考我使用的是C++ OpenCV 2.1.

在这个链接上描绘了我的坐标系和下面使用的测试数据.

% Input to solvePnP():
imagePoints =     [ 481, 831; % [x, y] format
                    520, 504;
                   1114, 828;
                   1106, 507]
objectPoints = [0.11, 1.15, 0; % [x, y, z] format
                0.11, 1.37, 0; 
                0.40, 1.15, 0;
                0.40, 1.37, 0]

% camera intrinsics for Logitech C910
cameraMat = [1913.71011, 0.00000,    1311.03556;
             0.00000,    1909.60756, 953.81594;
             0.00000,    0.00000,    1.00000]
distCoeffs = [0, 0, 0, 0, 0]

% output of solvePnP():
tVec = [-0.3515;
         0.8928; 
         0.1997]

rVec = [2.5279; …

我一直在尝试为使用惯性测量单元(IMU)和相机观察已知地标的机器人实施导航系统,以便在其环境中进行本地化.我选择了间接反馈卡尔曼滤波器(又名误差状态卡尔曼滤波器,ESKF)来做到这一点.我在扩展KF方面取得了一些成功.

我已经阅读了很多文本,我用来实现ESKF的两个是" 用于错误状态KF的四元数运动学 "和" 用于IMU相机校准的基于卡尔曼滤波器的算法 "(付费壁纸,google-able) .我使用的是第一个文本,因为它更好地描述了ESKF的结构,第二个是因为它包含了有关视觉测量模型的详细信息.在我的问题中,我将使用第一个文本中的术语:'名义状态','错误状态'和'真实状态'; 它指的是IMU积分器,卡尔曼滤波器,以及两者的组成(标称减去误差).

下图显示了我在Matlab/Simulink中实现的ESKF的结构; 如果您不熟悉Simulink,我将简要介绍该图表.绿色部分是标称状态积分器,蓝色部分是ESKF,红色部分是标称状态和错误状态的总和.'RT'块是'Rate Transitions',可以忽略.

我的第一个问题:这个结构是否正确？

我的第二个问题:测量模型的误差状态方程是如何导出的？ 在我的情况下,我尝试使用第二个文本的测量模型,但它没有用.

亲切的问候,