imu 里程计融合_MSCKF那些事（十一）算法改进4：融合直接观测

在实际应用过程中往往不只有视觉和IMU的融合，还有其他很多传感器（GPS、激光、里程计等），这些传感器数据和VIO进行融合能够极大提高VIO的精度和稳定性。融合的方式分为松耦合和紧耦合，常见的做法是将VIO看成一个独立的传感器与其他传感器进行松耦合，但有时为了提高VIO本身的精度也可以将部分传感器信息加入到VIO融合框架中。

状态向量中某些量可能会被其他传感器直接观测到，例如GPS可以给出

中的位置观测或速度观测，平面移动机器人有三个维度可以固定

，在MSCKF中可以对直接观测构建观测模型进行EKF更新。直接观测有两类：

一类是传感器直接测量到的观测：比如GPS位置、GPS速度、轮式里程计
一类是基于先验信息给出的观测：比如平面机器人可以假设
，此外还有零速检测（Zero-velocity UPdaTe，ZUPT），当检测到IMU处于静止状态时，可以认为速度为0，陀螺仪观测的均值作为陀螺仪的bias。

直接观测融合十分简单，构建观测模型

，然后套用EKF更新公式即可。

矩阵在观测状态的对应位置填1，其他填0，以融合速度观测为例，设{G}系下的IMU观测速度为

，误差

，EKF更新流程如下：

代码实现上可以做成一个通用的直接观测更新接口，输入参数：

observe_vec：直接观测向量，所有观测合并成一个向量
state_vec：直接观测向量中每一项对应的当前状态估计量
index_vec：直接观测向量中每一项对应协方差矩阵中的位置索引
noise_vec：直接观测向量中每一项的观测噪声

只要直接观测到状态向量中的任意项，都可以调用该API进行更新。

void updateVector(const VectorXd& observe_vec, const VectorXd& state_vec, const VectorXi& index_vec, const VectorXd& noise_vec){auto& P = m_state_server.state_cov;int D = index_vec.rows();MatrixXd H = MatrixXd::Zero(D, P.cols());MatrixXd HP = MatrixXd::Zero(D, P.cols());MatrixXd Q = MatrixXd::Zero(D, D);for(int i = 0; i < D; i++){H(i, index_vec(i)) = 1;HP.row(i) = P.row(index_vec(i));Q(i, i) = noise_vec(i);}// Compute the Kalman gain.MatrixXd S = HP * H.transpose() + Q;MatrixXd K_transpose = S.ldlt().solve(HP);MatrixXd K = K_transpose.transpose();// Compute the error of the state.VectorXd err = observe_vec - state_vec;VectorXd delta_x = K * err;if(updateState(delta_x, false)) updateStateCov(K, H, Q);}

imu 里程计融合_MSCKF那些事（十一）算法改进4：融合直接观测相关推荐

imu 里程计融合_地图影像与IMU及里程计集成的探测车定位
1.序列影像与IMU及里程计集成定位行星表面巡视探测中,探测车依靠IMU和里程计组合的航迹推算能够获取连续稳定的定位结果,然后行驶过程中的车轮打滑以及IMU长时间工作带来的偏移严重影响了定位精度.基 ...
imu 里程计融合_轮式里程计(Wheel Odometry)以及惯性导航(IMU)数据
对于众多机器人应用来说,自定位以及基于航位推测(Dead-reckoning, DR)估计的里程计信息是非常重要的.一般来说,要设计一个有效的里程计系统是十分困难的. 一.惯性导航单元(Inertia ...
最新开源Faster-LIO：快速激光IMU里程计
作者丨半闲居士@知乎来源丨https://zhuanlan.zhihu.com/p/468628910 编辑丨点云PCL 文章:Faster-LIO: Lightweight Tightly Cou ...
多传感器融合定位四-3D激光里程计其四：点云线面特征提取
多传感器融合定位四-3D激光里程计其四:点云线面特征提取 1. 点云线面特征提取 1.1 按线数分割 1.2 计算曲率(重要!) 1.3 按曲率大小筛选特征点 2. 基于线面特征的位姿变化 2.1 帧 ...
多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合多传感器融合定位理论基础文章跳转: 多传感器融合定位一-3D激光里 ...
多传感器融合定位二-3D激光里程计其二：NDT
多传感器融合定位二-3D激光里程计其二:NDT 1. 经典NDT 2. 计算方式 2.1 2D场景求解: 2.2 3D场景求解: 3. 其他 NDT Reference: 深蓝学院-多传感器融合多传 ...
基于ROS与C++，只利用IMU实现里程计和计步器，精确计算行走距离
老规矩,开门见山,先说说本篇文章实现了什么: 1.只使用IMU模块,实现了对于人行走距离的检测(精度在0.5m左右) 2.只使用IMU模块,通过峰值检测\机器学习两个方法,实现了一个精确的计步器背景 ...
python鱼眼图像识别_一种融合鱼眼图像与深度图像的动态环境视觉里程计方法与流程...
本发明涉及移动机器人同步定位与地图构建(SLAM)技术领域,尤其是适用于动态环境的基于鱼眼图像与深度图像的视觉里程计方法. 背景技术: 定位技术是移动机器人实现各项复杂任务的技术基础.里程计便是一类简 ...
lio-sam框架：后端里程计、回环、gps融合
lio-sam框架:后端里程计.回环.gps融合前言代码解析添加激光雷达帧间里程计因子添加GPS因子添加回环检测因子 isam 更新调整全局轨迹总结前言 LIO-SAM的全称是:Tig ...
【自动驾驶】视觉里程计
无人驾驶中的视觉里程计概念解读里程计的概念: 在里程计问题中,我们希望测量一个运动物体的轨迹.这可以通过许多不同的手段来实现.例如,我们在汽车轮胎上安装计数码盘,就可以得到轮胎转动的距离,从而得到汽 ...

imu 里程计融合_MSCKF那些事（十一）算法改进4：融合直接观测

imu 里程计融合_MSCKF那些事（十一）算法改进4：融合直接观测相关推荐

最新文章

热门文章