基于惯性传感器Roll角和Pitch角的MonoSLAM方法

单目视觉同步定位与地图创建MonoSLAM----误差累积问题
         顾照鹏的论文《基于部分惯性传感器信息的单目视觉同步定位与地图创建方法》提出基于Roll角和Pitch角的MonoSLAM法：
        先利用惯性传感器输出的Roll角和Pitch角系统标定， 然后将惯性传感器自身的Yaw作为系统状态向量的一个分量利用扩展卡尔曼滤波器（EKF）实时地估计状态向量， 进而实现实时鲁棒的SLAM。能够有效抑制误差累积问题。
惯性传感器（分为两类：角速率陀螺；线加速度计） 三轴方向角输出做辅助：

• 横滚角Roll-----精度高
•  俯仰角Pitch----精度高
•  偏航角Yaw-----精度低，不直接用

如何在未知环境中实时地确定用户头部位置和姿态？

        设计一个定位定姿模块：由摄像机+3DM-GX1惯性传感器组成。（3DM-GX1是陀螺仪姿态测量系统。）

       系统涉及世界坐标系，摄像机坐标系，固定在3DM-GX1上的3DM-GX1坐标系，地磁坐标系。他们之间的四个旋转矩阵满足一个式子。根据摄像机拍的图像和输出的Roll角和Pitch角可以标定两个旋转矩阵，基于它们，仅将Roll角和Pitch角作为姿态基准量，将3DM-GX1坐标系与地磁坐标系之间的Raw角作为未知状态分量，引入EKF中以提高系统稳定性。
        摄像机的状态向量定义为一个8维向量，摄像机光心位置，三轴速度，3DM-GX1坐标系与地磁坐标系之间的Yaw角及相应的角速度。环境地图特征定义为三维点特征。因此，整个MonoSLAM系统的状态向量x表示成一个（8+3N）维向量。通过不断迭代更新数据关联和状态向量，修正得到稳定的系统。

        实验用上述模块安在一个电动转台上，控制转台在65°范围内做往复圆周运动，同时拍一段视频，实时计算摄像机运动轨迹，以此为基准，论文中测试了所提出方法和另外三种方法，对比得到的3轴运动轨迹。验证得到所提出方法在苛刻条件下的有效性后，再做另一虚拟现实实验：在目标位置虚拟一坦克，将摄像机自由运动，计算出运动轨迹，初始目标位置不变，得证。