Weird result in mars_thl_example.cpp

[golbog]

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]

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 #6 (comment). 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]

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.