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

IMU测量模型（离散时间）

IMU测量模型：

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

其中，

离散时间的 Bias：

$$\begin{array}{rl}{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{array}$$

离散时间的 White Noise：

$$\begin{array}{rl}{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{array}$$

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

代码实现

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

std::random_device rd;
std::default_random_engine generator_(rd());
std::normal_distribution 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_;