首页 > 专利查询

lio-mapping及　VINS-Mono代码及理论推导（2）——pre_integration

lio-mapping 及　VINS-Mono代码及理论推导（2）—— pre_integration

最近在看港科⼤刚开源的LIO及VINS-Mono论⽂和代码，他们⼀部分代码都是相同的，闲暇时间整理⼀下。持续更新中（如果有时间）。。。

LIO

VINS

预积分部分核⼼函数为

MidPointIntegration(...)

MidPointIntegration 预积分函数

VINS_Mono

void midPointIntegration(double _dt,

const Eigen::Vector3d &_acc_0,const Eigen::Vector3d &_gyr_0,

const Eigen::Vector3d &_acc_1,const Eigen::Vector3d &_gyr_1,

const Eigen::Vector3d &delta_p,const Eigen::Quaterniond &delta_q,const Eigen::Vector3d &delta_v,

const Eigen::Vector3d &linearized_ba,const Eigen::Vector3d &linearized_bg,

Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,

Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg,bool update_jacobian)

{

//ROS_INFO("midpoint integration");

Vector3d un_acc_0 = delta_q *(_acc_0 - linearized_ba);

Vector3d un_gyr =0.5*(_gyr_0 + _gyr_1)- linearized_bg;

result_delta_q = delta_q *Quaterniond(1,un_gyr(0)* _dt /2,un_gyr(1)* _dt /2,un_gyr(2)* _dt /2);

Vector3d un_acc_1 = result_delta_q *(_acc_1 - linearized_ba);

Vector3d un_acc =0.5*(un_acc_0 + un_acc_1);

result_delta_p = delta_p + delta_v * _dt +0.5* un_acc * _dt * _dt;

result_delta_v = delta_v + un_acc * _dt;

result_linearized_ba = linearized_ba;

result_linearized_bg = linearized_bg;

if(update_jacobian)

{

Vector3d w_x =0.5*(_gyr_0 + _gyr_1)- linearized_bg;

Vector3d a_0_x = _acc_0 - linearized_ba;

Vector3d a_1_x = _acc_1 - linearized_ba;

Matrix3d R_w_x, R_a_0_x, R_a_1_x;

R_w_x<<0,-w_x(2),w_x(1),蓝牙定位系统

w_x(2),0,-w_x(0),

-w_x(1),w_x(0),0;

R_a_0_x<<0,-a_0_x(2),a_0_x(1),

a_0_x(2),0,-a_0_x(0),

-a_0_x(1),a_0_x(0),0;

R_a_1_x<<0,-a_1_x(2),a_1_x(1),

a_1_x(2),0,-a_1_x(0),

-a_1_x(1),a_1_x(0),0;

MatrixXd F = MatrixXd::Zero(15,15);

F.block<3,3>(0,0)= Matrix3d::Identity();

F.block<3,3>(0,3)=-0.25* RotationMatrix()* R_a_0_x * _dt * _dt +

-0.25* result_RotationMatrix()* R_a_1_x *(Matrix3d::Identity()- R_w_x * _dt)* _dt * _dt;

F.block<3,3>(0,6)= MatrixXd::Identity(3,3)* _dt;

F.block<3,3>(0,9)=-0.25*(RotationMatrix()+ result_RotationMatrix())* _dt * _dt;

F.block<3,3>(0,12)=-0.25* result_RotationMatrix()* R_a_1_x * _dt * _dt *-_dt;

F.block<3,3>(3,3)= Matrix3d::Identity()- R_w_x * _dt;

F.block<3,3>(3,12)=-1.0* MatrixXd::Identity(3,3)* _dt;

F.block<3,3>(6,3)=-0.5* RotationMatrix()* R_a_0_x * _dt +

-0.5* result_RotationMatrix()* R_a_1_x *(Matrix3d::Identity()- R_w_x * _dt)* _dt;

F.block<3,3>(6,6)= Matrix3d::Identity();

F.block<3,3>(6,9)=-0.5*(RotationMatrix()+ result_RotationMatrix())* _dt;

F.block<3,3>(6,12)=-0.5* result_RotationMatrix()* R_a_1_x * _dt *-_dt;

F.block<3,3>(9,9)= Matrix3d::Identity();

F.block<3,3>(12,12)= Matrix3d::Identity();

//cout<<"A"<<endl<<A<<endl;

MatrixXd V = MatrixXd::Zero(15,18);

V.block<3,3>(0,0)=0.25* RotationMatrix()* _dt * _dt;

V.block<3,3>(0,3)=0.25*-result_RotationMatrix()* R_a_1_x * _dt * _dt *0.5* _dt;

V.block<3,3>(0,6)=0.25* result_RotationMatrix()* _dt * _dt;

V.block<3,3>(0,9)= V.block<3,3>(0,3);

V.block<3,3>(3,3)=0.5* MatrixXd::Identity(3,3)* _dt;

V.block<3,3>(3,9)=0.5* MatrixXd::Identity(3,3)* _dt;

阵列天线V.block<3,3>(6,0)=0.5* RotationMatrix()* _dt;

V.block<3,3>(6,3)=0.5*-result_RotationMatrix()* R_a_1_x * _dt *0.5* _dt;

V.block<3,3>(6,6)=0.5* result_RotationMatrix()* _dt;

V.block<3,3>(6,9)= V.block<3,3>(6,3);

V.block<3,3>(9,12)= MatrixXd::Identity(3,3)* _dt;

V.block<3,3>(12,15)= MatrixXd::Identity(3,3)* _dt;

//step_jacobian = F;

//step_V = V;

jacobian = F * jacobian;

covariance = F * covariance * F.transpose()+ V * noise * V.transpose();

}

LIO-Mapping

对⽐代码发现　LIO-Mapping 与 VINS部分细节不同，注释中有提及。

void MidPointIntegration(double dt,

const Vector3d &acc0,const Vector3d &gyr0,

const Vector3d &acc1,const Vector3d &gyr1,

const Vector3d &delta_p,const Quaterniond &delta_q,

const Vector3d &delta_v,const Vector3d &linearized_ba,

const Vector3d &linearized_bg, Vector3d &result_delta_p,

Quaterniond &result_delta_q, Vector3d &result_delta_v,

Vector3d &result_linearized_ba, Vector3d &result_linearized_bg,

bool update_jacobian){

//ROS_DEBUG("midpoint integration");

// NOTE: the un_acc here is different from the un_acc in the Estimator

Vector3d un_acc_0 = delta_q *(acc0 - linearized_ba);

Vector3d un_gyr =0.5*(gyr0 + gyr1)- linearized_bg;

result_delta_q = delta_q *Quaterniond(1,un_gyr(0)* dt /2,un_gyr(1)* dt /2,un_gyr(2)* dt /2);

Vector3d un_acc_1 = result_delta_q *(acc1 - linearized_ba);

Vector3d un_acc =0.5*(un_acc_0 + un_acc_1);

result_delta_p = delta_p + delta_v * dt +0.5* un_acc * dt * dt;

result_delta_v = delta_v + un_acc * dt;

result_linearized_ba = linearized_ba;

result_linearized_bg = linearized_bg;

if(update_jacobian){

Vector3d w_x =0.5*(gyr0 + gyr1)- linearized_bg;

Vector3d a_0_x = acc0 - linearized_ba;

Vector3d a_1_x = acc1 - linearized_ba;

Matrix3d R_w_x, R_a_0_x, R_a_1_x;

R_w_x <<0,-w_x(2),w_x(1),

R_w_x << 0, -w_x (2), w_x (1), w_x (2), 0, -w_x (0), -w_x (1), w_x (0), 0;

R_a_0_x << 0, -a_0_x (2), a_0_x (1), a_0_x (2), 0, -a_0_x (0), -a_0_x (1), a_0_x (0), 0;

R_a_1_x << 0, -a_1_x (2), a_1_x (1), a_1_x (2), 0, -a_1_x (0), -a_1_x (1), a_1_x (0), 0; // NOTE: F = Fd = \Phi = I + dF*dt MatrixXd F = MatrixXd ::Zero (15, 15); F .block <3, 3>(0, 0) = Matrix3d ::Identity ();

F .block <3, 3>(0, 3) = -0.25 * delta_q .toRotationMatrix () * R_a_0_x * dt * dt +

-0.25 * result_delta_q .toRotationMatrix () * R_a_1_x * (Matrix3d ::Identity () - R_w_x * dt ) * dt * dt ; F .block <3, 3>(0, 6) = MatrixXd ::Identity (3, 3) * dt ;

F .block <3, 3>(0, 9) = -0.25 * (delta_q .toRotationMatrix () + result_delta_q .toRotationMatrix ()) * dt * dt ;// F.block<3, 3>(0, 12) = -0.25 * result_RotationMatrix() * R_a_1_x * dt * dt * -dt;

F .block <3, 3>(0, 12) = -0.1667 * result_delta_q .toRotationMatrix () * R_a_1_x * dt * dt * -dt ;//0.1667与VINS 不同 F .block <3, 3>(3, 3) = Matrix3d ::Identity () - R_w_x * dt ; F .block <3, 3>(3, 12) = -1.0 * MatrixXd ::Identity (3, 3) * dt ;

F .block <3, 3>(6, 3) = -0.5 * delta_q .toRotationMatrix () * R_a_0_x * dt +

-0.5 * result_delta_q .toRotationMatrix () * R_a_1_x * (Matrix3d ::Identity () - R_w_x * dt ) * dt ; F .block <3, 3>(6, 6) = Matrix3d ::Identity ();

F .block <3, 3>(6, 9) = -0.5 * (delta_q .toRotationMatrix () + result_delta_q .toRotationMatrix ()) * dt ; F .block <3, 3>(6, 12) = -0.5 * result_delta_q .toRotationMatrix () * R_a_1_x * dt * -dt ; F .block <3, 3>(9, 9) = Matrix3d ::Identity (); F .block <3, 3>(12, 12) = Matrix3d ::Identity (); //cout<<"A"<<endl<<A<<endl; // NOTE: V = Fd * G_c

// FIXME: verify if it is right, the 0.25 part MatrixXd V = MatrixXd ::Zero (15, 18);

// V.block<3, 3>(0, 0) = 0.25 * RotationMatrix() * dt * dt;

V .block <3, 3>(0, 0) = 0.5 * delta_q .toRotationMatrix () * dt * dt ;//0.5与VINS 不同

V .block <3, 3>(0, 3) = 0.25 * -result_delta_q .toRotationMatrix () * R_a_1_x * dt * dt * 0.5 * dt ;// V.block<3, 3>(0, 6) = 0.25 * result_RotationMatrix() * dt * dt;

V .block <3, 3>(0, 6) = 0.5 * result_delta_q .toRotationMatrix () * dt * dt ;//0.5与VINS 不同 V .block <3, 3>(0, 9) = V .block <3, 3>(0, 3);

V .block <3, 3>(3, 3) = 0.5 * MatrixXd ::Identity (3, 3) * dt ; V .block <3, 3>(3, 9) = 0.5 * MatrixXd ::Identity (3, 3) * dt ; V .block <3, 3>(6, 0) = 0.5 * delta_q .toRotationMatrix () * dt ;

lora通信V .block <3, 3>(6, 3) = 0.5 * -result_delta_q .toRotationMatrix () * R_a_1_x * dt * 0.5 * dt ; V .block <3, 3>(6, 6) = 0.5 * result_delta_q .toRotationMatrix () * dt ; V .block <3, 3>(6, 9) = V .block <3, 3>(6, 3);

V .block <3, 3>(9, 12) = MatrixXd ::Identity (3, 3) * dt ; V .block <3, 3>(12, 15) = MatrixXd ::Identity (3, 3) * dt ; //step_jacobian = F; //step_V = V;

jacobian_ = F * jacobian_;

covariance_ = F * covariance_ * F .transpose () + V * noise_ * V .transpose (); } }

公式推导

下⾯是两帧雷达数据之间所以IMU数据的位置、速度、旋转量的积分公式

p =j p +i [v Δt +k =i ∑

j −1

k 0.25(R (a −k i

k b )+a k R (a −k +1i k b ))Δt ]

a k 2(1)

不考虑噪声的情况下，每来⼀帧imu数据，其状态转移如下

误差状态加⼊噪声影响，加速度和陀螺仪的测量噪声为，。加速度和陀螺仪偏置为随机游⾛噪声，，，针对MidPointIntegration，其还需要增加时刻的测量噪声，也即其噪声矩阵为。

v =j v +

i 0.5[R (a −k =i ∑

j −1

k i

k b )Δt +a k R (a −k +1i

智辅

k b )Δt ]

a k (2)

q =j q ⊗

i δq =k =i ∏

j −1

k q ⊗

i k =i ∏

j −1

[

Δt (−b )

ω+ωk k +1g k 1

]

(3)

q =k +1q ⊗k [

Δt (−b )

ω+ωk k +1g k 1

]

(4)

p =k +1p +k v Δt +k 0.25(R (a −k k b )+a k R (a −k +1k +1b ))Δt a k +12

(5)

v =k +1v +k 0.5(R (a −k k b )+a k R (a −k +1k +1b ))Δt a k +1(6)

b =a k +1b a k (7)

b =g k +1b g k

(8)

δX =(δp ,δθ,δv ,δb ,δb )

a g n ∼a N (0,σ)a 2

n ∼w N (0,σ)w 2

n ∼

b a N (0,σ)b a 2

点火加热装置

n ∼b w N (0,σ)b w 2

k +1Q =⎣⎢⎢⎢⎢⎢⎢⎡n a 000000n w 000000n a 0000

0n w 00

0000n b a 000000n b w ⎦⎥⎥⎥⎥⎥⎥⎤

根据式(4)，⾸先求，定义：更新为：

假设加加速度恒定，再求 :

得到:

再求求最后更新偏置,δθ

˙δq =[δθ

211]=δθ˙−[−2

ω+ωk k +1

b ]δθ−g k ×δb +g 0.5(n +w k n )　w k +1(9)

δθδθ←new (I −[(−2

ω+ωk k +1

b )]Δt )δθ−g k ×Δt δb +g 0.5Δt (n +w k n )w k +1(10)

δv ˙=δv ˙==−0.5(R [a −b ]δθ+R [a −b ]δθ)

k k a k ×k +1k +1a k +1×new +0.5(R +R )δb +0.5(R n +R n )

k k +1a k a k k +1a k +1−0.5(R [a −b ]δθ+R [a −b ]((I −[(−b )]Δt )δθ−Δt δb +0.5Δt (n +n ))k k a k ×k +1k +1a k +1×2ω+ωk k +1

g k ×g w k w k +1+0.5(R +R )δb +0.5(R n +R n )

k k +1a k a k k +1a k +1−0.5(R [a −b ]+R [a −b ]((I −[(−b )]Δt ))δθk k a k ×k +1k +1a k +1×2ω+ωk k +1g k ×−0.5(R +R )δb +0.5(R n +R n )

混凝土泵送剂

k k +1a k a k k +1a k +1+0.5R [a −b ]Δt δb −0.25R [a −b ]Δt (n +n )k +1k +1a k +1×g k +1k +1a k +1×w k w k +1(11)

δv new δv ←new δv +Δt

δv ˙(12)

δp

˙=δp ˙=0.5(δv +δv )new δv +0.5δv

˙(13)

δp

δp ←new δp +Δt

δp ˙(14)

δb a δb g

δb ←a

new

δb +a n Δt b a (15)

δb ←g

new

δb +g n Δt

b w (16)

本文发布于:2024-09-22 05:42:29，感谢您对本站的认可！

本文链接：https://www.17tex.com/tex/4/97972.html

上一篇：Tensorflow：分类模型评估

下一篇：KNN分类算法及MATLAB程序与结果

标签：噪声加速度代码部分测量数据陀螺仪

留言与评论（共有 0 条评论）

lio-mapping及 VINS-Mono代码及理论推导（2）——pre_integration

lio-mapping及　VINS-Mono代码及理论推导（2）——pre_integration