从零开始手写vio-ch2-imu数据仿真
程序员文章站
2022-04-16 23:36:10
...
一、利用欧拉积分和中指=值积分来计算imu数据，并绘制轨迹，与给定的轨迹方程进行比较
利用欧拉积分或者中值积分计算imu数据
核心代码imu.cpp源码如下：
//
// Created by hyj on 18-1-19.
//
#include <random>
#include "imu.h"
#include "utilities.h"
// euler2Rotation: body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double yaw = eulerAngles(2);
double cr = cos(roll); double sr = sin(roll);
double cp = cos(pitch); double sp = sin(pitch);
double cy = cos(yaw); double sy = sin(yaw);
//RIb=RbI^−1=RbI^T
Eigen::Matrix3d RIb;//这里对应的是公式 是从i系到b系的旋转矩阵的转置
//i系到b系的旋转矩阵的公式见ppt 就是Rbi=R(yaw,pith,roll)
RIb<< cy*cp , cy*sp*sr - sy*cr, sy*sr + cy* cr*sp,
sy*cp, cy *cr + sy*sr*sp, sp*sy*cr - cy*sr,
-sp, cp*sr, cp*cr;
return RIb;
}
//变换矩阵--将欧拉角速度从I坐标变到body坐标
Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double cr = cos(roll); double sr = sin(roll);
double cp = cos(pitch); double sp = sin(pitch);
//公式见ppt
Eigen::Matrix3d R;
R<< 1, 0, -sp,
0, cr, sr*cp,
0, -sr, cr*cp;
return R;
}
IMU::IMU(Param p): param_(p)
{
gyro_bias_ = Eigen::Vector3d::Zero();
acc_bias_ = Eigen::Vector3d::Zero();
}
//加入噪声
void IMU::addIMUnoise(MotionData& data)
{
std::random_device rd;
std::default_random_engine generator_(rd());
std::normal_distribution<double> noise(0.0, 1.0);
Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;
Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();
data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;
// gyro_bias update
Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
data.imu_gyro_bias = gyro_bias_;
// acc_bias update
Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
data.imu_acc_bias = acc_bias_;
}
得到body坐标系下的IMU测量值
MotionData IMU::MotionModel(double t)
{
MotionData data;
// param
float ellipse_x = 15;//椭圆长轴
float ellipse_y = 20;//椭圆短轴
float z = 1; // z轴做sin运动
float K1 = 10; // z轴的正弦频率是x，y的k1倍
float K = M_PI/ 10; // 20 * K = 2pi 由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周
// translation
// twb: body frame in world frame
// position(t)=[ellipsex∗cos(K∗t)+5,ellipsey∗sin(K∗t)+5,z∗sin(K1∗K∗t)+5]
//设定质量快运动轨迹方程（以例程中椭圆为例）
Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5, z * sin( K1 * K * t ) + 5);
// 上面知道了位置矢量方程 一次求导 求出质量块速度方程
Eigen::Vector3d dp(- K * ellipse_x * sin(K*t), K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t)); // position导数 in world frame
double K2 = K*K;//
//对质量块速度方程再求一次导，得到质量块加速度方程
Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t), -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t)); // position二阶导数
// Rotation
double k_roll = 0.1;
double k_pitch = 0.2;
/**
* 在东北天坐标系中，yaw，pitch，roll对应的是绕载体坐标系的z，y，x轴。在北东地坐标系中，yaw，pitch，roll对应的是绕载体坐标系的y, x, z轴。在IMU仿真程序中，我们采用东北天坐标系，欧拉角顺规为yaw，pitch，roll。具体的值为
ϑ \vartheta ϑ = （ k r o l l ∗ c o s ( t ) , k p i t c h ∗ s i n ( t ) , K ∗ t ） T （kroll * cos(t) , kpitch * sin(t) , K*t）^T （kroll∗cos(t),kpitch∗sin(t),K∗t）^T
**/
//质量块的欧拉角 是body系在不同时刻相对于惯性系所转角度
Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t ); // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]
// 上面的质量块欧拉角求导得到欧拉角速度
Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K); // euler angles 的导数
// Eigen::Vector3d eulerAngles(0.0,0.0, K*t ); // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
// Eigen::Vector3d eulerAnglesRates(0.,0. , K); // euler angles 的导数
Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles); //得到从b坐标系到I坐标系的旋转矩阵(这里I坐标系就是W坐标系？？) 这里可以参考从零手写vio-ch2笔记 // body frame to world frame
//eulerRates2bodyRates(eulerAngles)//b坐标系到I坐标系的角速度的变换矩阵 这里可以参考从零手写vio-ch2笔记
Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates; // euler rates trans to body gyro
Eigen::Vector3d gn (0,0,-9.81); //东北天 导航系 // gravity in navigation frame(ENU) ENU (0,0,-9.81) NED(0,0,9,81)
Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp - gn ); // imu_acc是测量值 ddp是imu得到的值 Rbw * Rwn * gn = gs
data.imu_gyro = imu_gyro;//角速度
data.imu_acc = imu_acc;//加速度
data.Rwb = Rwb;//旋转矩阵 从body 坐标系到world(I系？)
data.twb = position;//位置，也可理解为平移
data.imu_velocity = dp;//速度 速度就是轨迹方程的导数
data.timestamp = t;//时间戳
return data;
}
读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，比对积分出来的轨迹与自己设定出来的轨迹是不是一样的
用来验证数据以及模型的有效性。
void IMU::testImu(std::string src, std::string dist)
{
std::vector<MotionData>imudata;
LoadPose(src,imudata);
std::ofstream save_points;
save_points.open(dist);
//初始化
double dt = param_.imu_timestep;//理解为离散的采样时间吧
Eigen::Vector3d Pwb = init_twb_; // position : from imu measurements
Eigen::Quaterniond Qwb(init_Rwb_); // quaterniond: from imu measurements
Eigen::Vector3d Vw = init_velocity_; // velocity : from imu measurements
Eigen::Vector3d gw(0,0,-9.81); // ENU frame
Eigen::Vector3d temp_a;
Eigen::Vector3d theta;
//读取imu数据
for (int i = 1; i < imudata.size(); ++i) {
MotionData imupose = imudata[i];
/// imu 动力学模型 欧拉积分 加入bias
/* //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = (imupose.imu_gyro-imupose.imu_gyro_bias) * dt /2.0;
//这里的dq就是离散时间段的旋转小量
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();
// 这一部分对应ppt欧拉法公式
Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc-imupose.imu_acc_bias) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;//imu的速度加上旋转
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;*/
/// 中值积分 加入噪声
Eigen::Quaterniond Qwbk,Qwbk1,dq;//dq是旋转小量
Eigen::Vector3d midOmega;
Eigen::Vector3d dtheta_half;
midOmega=(imudata[i].imu_gyro-imudata[i-1].imu_gyro_bias+imudata[i-1].imu_gyro-imudata[i-1].imu_gyro_bias)/2;
dtheta_half=midOmega*dt/2.0;//这就是公式的旋转小量的 1/2* w *dt
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
dq.normalize();//将小量四元数单位化
Qwbk=Qwb;
Qwbk1=Qwbk*dq;//得到下一时刻的四元数-旋转
Eigen::Vector3d acc_w =0.5*( Qwbk * (imudata[i-1].imu_acc-imudata[i-1].imu_acc_bias) + gw +Qwbk1 * (imupose.imu_acc-imudata[i-1].imu_acc_bias) + gw );
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;
Qwb = Qwbk1;
// 按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
save_points<<imupose.timestamp<<" "
<<Qwb.w()<<" "
<<Qwb.x()<<" "
<<Qwb.y()<<" "
<<Qwb.z()<<" "
<<Pwb(0)<<" "
<<Pwb(1)<<" "
<<Pwb(2)<<" "
<<Qwb.w()<<" "
<<Qwb.x()<<" "
<<Qwb.y()<<" "
<<Qwb.z()<<" "
<<Pwb(0)<<" "
<<Pwb(1)<<" "
<<Pwb(2)<<" "
<<std::endl;
}
std::cout<<"test end"<<std::endl;
}
没有加噪声
欧拉法：
中值法：
加噪声
欧拉法：中值法：
实验总结
在没有加入噪声之前，可以看出欧拉法的误差比较大，中值法基本与轨迹重合。
加入噪声后，有噪声的数据随着时间推移，误差越来越大，直到完全脱离轨迹。
二、Allan方差标定
参考博客：Allan