【IMU数据与GPS融合的预积分方法】基于流形的IMU预积分，用于高效的视觉惯性最大后验估计、SE3姿势区分为IMU（Matlab代码实现）

 💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

 ⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、文章下载

---

 ⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

     或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

参考文献：

摘要—最近在单目视觉惯性导航（VIN）方面的研究结果表明，基于优化的方法在准确性方面优于滤波方法，因为其具有对过去状态重新线性化的能力。然而，这种改进是以增加计算复杂性为代价的。本文通过在选定的关键帧之间预积分惯性测量来解决这个问题。预积分使我们能够将数百个惯性测量准确地总结为单个相对运动约束。我们的第一个贡献是一个预积分理论，正确处理旋转群的流形结构，并仔细处理不确定性传播。这些测量在一个局部坐标系中集成，消除了在线性化点变化时重复集成的需要，同时为延迟的偏差校正留下了机会。第二个贡献是展示预积分的IMU模型可以无缝地集成到因子图的统一框架中的视觉惯性流程中。这使得可以使用无结构模型进行视觉测量，进一步加速计算。第三个贡献是对我们的单目VIN流程进行了广泛评估：实验结果证实我们的系统非常快速，并且在与竞争对手的最新滤波和优化算法以及Google Tango等现成系统的准确性方面表现出色。

根据参考文献中的插值将一系列SE3姿势区分为IMU数据：Spline Fusion：一种连续时间表示，用于视觉惯性融合，适用于滚动快门相机。[PDF] Spline Fusion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras | Semantic Scholar 

📚2 运行结果

部分代码：

clear;clc;
% global attiCaculator;

randn('seed',0);
step = 0.01;
start_time = 0;
end_time = 50;
tspan = [start_time:step:end_time]';
N = length(tspan);
Ar = 10;
r = [Ar*sin(tspan) Ar*cos(tspan) 0.5*tspan.*tspan]; % pose of GPS 鈥斺�� global pose
v = [Ar*cos(tspan) -Ar*sin(tspan) tspan];
acc_inertial = [-Ar*sin(tspan) -Ar*cos(tspan) ones(N,1)];  % global acceleration

atti = [0.1*sin(tspan) 0.1*sin(tspan) 0.1*sin(tspan)];  % Rotation: pitch roll yaw || x y z
Datti = [0.1*cos(tspan) 0.1*cos(tspan) 0.1*cos(tspan)];
g = [0 0 -9.8]';
gyro_pure = zeros(N,3);
acc_pure = zeros(N,3);

a = wgn(N,1,1)/5;
b = zeros(N,1);
b(1) = a(1)*step;
%% generate imu data
for iter = 1:N
    A = AttitudeBase.Datti2w(atti(iter,:));
    gyro_pure(iter,:) = Datti(iter,:)*A';
    cnb = AttitudeBase.a2cnb(atti(iter,:));
    acc_pure(iter,:) = cnb*(acc_inertial(iter,:)' - g);
end
% state0 = zeros(10,1);  
% state0(7) = 1;

% add noise and bias (0.5 m/s^2 & 1 degree)
acc_noised = acc_pure + 0.1*randn(N,3) + 0.5*ones(N,3);
gyro_noised = gyro_pure + (randn(N,3)*2 + ones(N,3))/180*pi;
accCov = 0.01*eye(3);
gyroCov = (2/180*pi)^2*eye(3);

%% preintegration
imuPara = IMUPara(accCov,gyroCov,ones(3),ones(3));  % initial imuPara with ones?

PIM = PreintegrateMeasurement();
for i = 1:100   % the former 100 imu data
    PIM = PIM.Preintegrate(acc_noised(i,:)',gyro_noised(i,:)',imuPara,step);
end

si =  NavState(zeros(3,1),r(1,:)',v(1,:)',zeros(3,1),zeros(3,1));   % initial the NavState
sj = si.predict(PIM);   % update the NavState by imu preintegraion

% sj = si;
% cov = zeros(15,15);
% cov(1:9,1:9) = PIM.cov_;
% cov(10:15,10:15) = 0.1*eye(6);
% infor = eye(15)/cov;
% oriMeas = SO3.log(AttitudeBase.a2cnb(atti(100,:))');

cov = zeros(12,12);
cov(1:9,1:9) = PIM.cov_;
cov(10:12,10:12) = 0.1*eye(3);
infor = eye(12)/cov;
for i = 1:2
    %     [ err,J ] = IMUErrorJacobian.GPSAndOri( si,sj,PIM,r(100,:)',oriMeas );
    [ err,J ] = IMUErrorJacobian.GPS( si,sj,PIM,r(100,:)' );    %% get residual error & uncomplete Jaccobian
    fprintf('err: %f\n',norm(err));
    %     if norm(err)<0.00001
    %         break;
    %     end
    delta = -(J'*infor*J)\J'*infor*err;
    %     fprintf('delta: %f\n',norm(delta))
    
    sj = sj.update(delta);
    si.ba_ = sj.ba_;
    si.bg_ = sj.bg_;
end

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

🌈4 Matlab代码、文章下载