IMU数据仿真公式推导及代码实现

晨光ABC

已于 2022-04-08 12:40:13 修改

阅读量2k

点赞数

分类专栏： INS 文章标签： IMU

于 2020-07-10 22:02:56 首次发布

本文链接：https://blog.csdn.net/u011178262/article/details/107270720

版权

INS 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

Overview

欢迎访问 https://spatial-ai.net/imu-data-sim.html，持续更新

IMU测量模型（离散时间）

IMU测量模型：

$\begin{aligned} {\omega}_m &= \omega + b_{gd} + n_{gd} \\ a_m &= a + b_{ad} + n_{ad} \end{aligned}$

其中，

离散时间的 Bias：

$\begin{aligned} b_{gd}[k] &= b_{gd}[k-1] + \sigma_{bgd} \cdot w[k] \\ &= b_{gd}[k-1] + \sigma_{bg} \sqrt{\Delta t} \cdot w[k] \\ b_{ad}[k] &= b_{ad}[k-1] + \sigma_{bad} \cdot w[k] \\ &= b_{ad}[k-1] + \sigma_{ba} \sqrt{\Delta t} \cdot w[k] \end{aligned}$

离散时间的 White Noise：

$\begin{aligned} n_{gd} &= \sigma_{gd} \cdot w[k] \\ &= \sigma_{g} \frac{1}{\sqrt{\Delta t}} \cdot w[k] \\ n_{ad} &= \sigma_{ad} \cdot w[k] \\ &= \sigma_{a} \frac{1}{\sqrt{\Delta t}} \cdot w[k] \end{aligned}$

其中， $\sim \mathcal{N}(0,1)$ ， $\Delta t$ 为采样时间。

代码实现

参考贺一家博士的代码（HeYijia/vio_data_simulation）

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_;