search

aau-cns / mars_lib

MaRS: A Modular and Robust Sensor-Fusion Framework
Other
258 stars 34 forks source link

Weird result in `mars_thl_example.cpp` #12

Open golbog opened 10 months ago

golbog commented 10 months ago

Running the mars_thl_example (version: 0.1.0-1c08c9024dc3) I get the following output:

Trajectory File: traj.csv
Pose File: pose_sensor_1.csv
IMU Noise Parameter: 
imu_n_w:     [0.013 0.013 0.013  ]
imu_n_bw:    [0.0013 0.0013 0.0013  ]
imu_n_a:     [0.083 0.083 0.083  ]
imu_n_ba:    [0.0083 0.0083 0.0083  ]
Created: [IMU] Sensor
Created: [Pose] Sensor
Created: Buffer (Size=300)
Created: Buffer (Size=100)
Created: CoreLogic - Using MaRS Version: 0.1.0-1c08c9024dc3
Warning: Core is not initialized yet. Measurement is stored but not processed
States Initialized:
p_wi:   [ 0.0000000000 0.0000000000 5.0000000000 ]
v_wi:   [ 0.0000000000 0.0000000000 0.0000000000 ]
q_wi:   [ 1.0000000000 0.0000000000 0.0000000000 0.0000000000 ]
b_w:    [ 0.0000000000 0.0000000000 0.0000000000 ]
b_a:    [ 0.0000000000 0.0000000000 0.0000000000 ]
w_m:    [  0.0112082519  0.0041435909 -0.0170691543 ]
a_m:    [ 0.0446263726 0.1522124562 9.6225157105 ]

Info: Filter was initialized
Info: Initialized [Pose] with [Given] Calibration at t=0.0200000000 
Last State:
p_wi:   [ -0.0046029310  0.0059225648  0.2394782674 ]
v_wi:   [ -0.0132090009  0.0135288417 -0.0003068795 ]
q_wi:   [ 0.9662831594  0.1198218208 -0.2223775546  0.0498779543 ]
b_w:    [ -0.0020980574  0.0021448086  0.0019942386 ]
b_a:    [ -0.0076526597 -0.0083927667  0.0015927020 ]
w_m:    [ -1.2525263620 -0.2494051525 -1.5072852095 ]
a_m:    [ 4.2713690019 2.0292181981 8.5375555121 ]

p_wi error [m]: [20946.8127698077  3518.0459166914 -8630.9125678099 ]
v_wi error [m/s]: [-15.9379285640  20.4974130584 -11.4554613456 ]
q_wi error [w,x,y,z]: [0.9624679859 -0.1016644688  0.1740093591 -0.1817703354 ]

The error values are abnormally large as if the filtering wasn't set up/working properly, or am I doing something wrong?

I've noticed this is already referenced in the #6, where the out-of-the-box running was fixed. But wouldn't it also make sense that the default provided GT values in the example are the correct ones for the bundled dataset?

Chris-Bee commented 10 months ago

Thanks for reaching out. Yes the GT values as well as the initial condition should be corrected. I'll fix this shortly.

I'm happy you saw the related issue https://github.com/aau-cns/mars_lib/issues/6#issuecomment-1580568923. Your initialization of the position is still set to p_wi: [ 0.0 0.0 5.0 ]. Until I fixed that, this value should be set to p_wi: [ 0.0 0.0 0.0 ] by hand.

Hope this helps.

golbog commented 10 months ago

Thank you for the response!

Feel free to close the issue when the fix is merged. Additionally, if you want I can provide a PR with the fix.