Curiosity about prior bias

[minwoo0611]

Hello, I have been studying SLAM and have a question about prior bias in the code.

I looked up the paper_metrics.cpp, and it compares the affect of bias term.
Exploiting the Taylor first order expansion, it could successfully correct the position and rotation.
However, if we exploited prior bias instead of Taylor expansion, its errors become larger.

I tested using this code.

celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
celib::PreintMeas preint = imu_preint.get(0,0);
std::vector acc = {acc_bias_norm[ibf], acc_bias_norm[ibf], acc_bias_norm[ibf]}
prior.acc_bias = acc;
celib::ImuPreintegration imu_preint2(data, start_t, t, preint_opt, prior);
celib::PreintMeas preint_corrected = imu_preint2.get(0,0);

The results :

\scriptsize Bias norm & \scriptsize 0.01 & \scriptsize 0.05 & \scriptsize 0.1 & \scriptsize 0.2 & \scriptsize 0.4 & \scriptsize 0.6 & \scriptsize 0.8 & \scriptsize 1

\scriptsize Slow Pos er. & \scriptsize 0.312 & \scriptsize 0.466 & \scriptsize 1.17 & \scriptsize 2.12 & \scriptsize 2.31 & \scriptsize 4.24 & \scriptsize 4.61 & \scriptsize 8.47 & \scriptsize 9.21 & \scriptsize 17 & \scriptsize 13.8 & \scriptsize 25.4 & \scriptsize 18.4 & \scriptsize 33.9 & \scriptsize 23 & \scriptsize 42.4
\scriptsize Fast Pos er. & \scriptsize 0.106 & \scriptsize 0.134 & \scriptsize 0.253 & \scriptsize 0.473 & \scriptsize 0.491 & \scriptsize 0.932 & \scriptsize 0.982 & \scriptsize 1.86 & \scriptsize 1.97 & \scriptsize 3.71 & \scriptsize 2.96 & \scriptsize 5.57 & \scriptsize 3.95 & \scriptsize 7.42 & \scriptsize 4.94 & \scriptsize 9.28

I will be waiting for your reply.

[minwoo0611]

Also, I have one more question.
When I looked up the simulation data, it has the values like

Acc :
t = 49.7: -123.265 5.65836 -96.4408
t = 49.71: -115.29 -14.1178 -108.216
t = 49.72: -103.836 -34.6069 -116.738
t = 49.73: -89.3185 -54.8485 -121.755
t = 49.74: -72.1834 -74.0596 -123.199
t = 49.75: -52.8242 -91.2382 -121.221
t = 49.76: -31.7813 -105.713 -115.992
t = 49.77: -9.74958 -116.703 -108.002
t = 49.78: 12.5941 -123.803 -97.7883
t = 49.79: 34.6741 -126.724 -85.849
t = 49.8: 55.8514 -125.284 -72.8122
t = 49.81: 75.5245 -119.646 -59.3747
t = 49.82: 93.1892 -109.877 -45.9319
t = 49.83: 108.328 -96.5401 -33.0857
t = 49.84: 120.615 -80.0337 -21.1201
t = 49.85: 129.778 -61.1537 -10.4706
t = 49.86: 135.662 -40.3639 -1.21431
t = 49.87: 138.27 -18.6247 6.59475
t = 49.88: 137.537 3.51647 12.9185
t = 49.89: 133.62 25.1811 18.0031

Gyr:
t = 49.7: 5.06803 0.331617 -3.37361
t = 49.71: 5.09259 0.105209 -3.41746
t = 49.72: 5.1017 -0.122764 -3.44323
t = 49.73: 5.10117 -0.338303 -3.45383
t = 49.74: 5.08608 -0.549359 -3.44729
t = 49.75: 5.05946 -0.754124 -3.42094
t = 49.76: 5.02358 -0.948433 -3.38624
t = 49.77: 4.96798 -1.13129 -3.3338
t = 49.78: 4.90661 -1.3028 -3.27179
t = 49.79: 4.82111 -1.46483 -3.20401
t = 49.8: 4.72532 -1.61724 -3.12596
t = 49.81: 4.61642 -1.75594 -3.04137
t = 49.82: 4.49547 -1.88843 -2.95713
t = 49.83: 4.35964 -2.0071 -2.8605
t = 49.84: 4.20899 -2.1156 -2.76584
t = 49.85: 4.04043 -2.21341 -2.67072
t = 49.86: 3.85893 -2.30396 -2.5735
t = 49.87: 3.66713 -2.39031 -2.47912
t = 49.88: 3.46429 -2.46272 -2.38664
t = 49.89: 3.24673 -2.52821 -2.29288

As I know, the acceleromter data in real world should be relatively lower than simulation data(such as gravity = 9.8m/s^2).
Also, gyroscope data seems larger than real world.

For example, IMU data of in2laama dataset has such value.
angular_velocity:
x: 0.00448878901079
y: -0.00980123877525
z: 0.0122299427167
angular_velocity_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
linear_acceleration:
x: 0.387243926525
y: -2.18475651741
z: 9.643907547
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

Is the units of data used in code are not same with (acc : m/s^2, gyr : rad/s)?

[clegenti]

Hi

Sorry for the late answer, the notifications must have slipped through...
You probably moved on since, but here is my answer.

Regarding the biases, from the snippet of code you provided, it seems that you are not actually correcting post-integration for the biases.
The "prior feature" is not correcting, it is basically just removing whatever given value from the raw IMU data.
To correct for your specific bias you need to do something like

preint_corrected.delta_R = preint.delta_R * celib::ExpMap(preint.d_delta_R_d_bw * (-bias_vec));
preint_corrected.delta_v = preint.delta_v + (preint.d_delta_v_d_bw * (-bias_vec));
preint_corrected.delta_p = preint.delta_p + (preint.d_delta_p_d_bw * (-bias_vec));

Regarding the values of acceleration, these numbers are indeed quite high. This is to show that even in extremely aggressive motion our method still performs well (less on the accelerometer but high angular velocities are common with handheld devices).
You can easily change the parameters of the motion sines to reflect better your target application in sensor_input/imu_simulator.cpp.