About the example of mars_thl

[dmkr001]

Hi authors, thanks for your work!
I tried running the example provided in your code, but it prompted that the traj.csv and pose_sensor_1.csv file are missing the headers. So I added headers in these files in the order of variables written in the code. And I got the result below:
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.0446263726 0.1522124562 9.6225157105 ]
a_m: [ 0.0112082519 0.0041435909 -0.0170691543 ]

Info: Filter was initialized
Info: Initialized [Pose] with [Given] Calibration at t=0.0200000000
Last State:
p_wi: [ 0.4011546192 0.0754448151 -0.0838039919 ]
v_wi: [ 0.5411666457 -1.3466671535 -1.6892457075 ]
q_wi: [ -0.2994304785 -0.5357607869 -0.1786263497 0.7690217129 ]
b_w: [ 1.5799786506 -0.0864311147 8.9297938184 ]
b_a: [ -1.3796940856 0.3320506584 -4.6791725388 ]
w_m: [ 4.2713690019 2.0292181981 8.5375555121 ]
a_m: [ -1.2525263620 -0.2494051525 -1.5072852095 ]

p_wi error [m]: [20947.2185273579 3518.1154389416 -8631.2358500692 ]
v_wi error [m/s]: [-15.3835529174 19.1372170633 -13.1444001735 ]
q_wi error [w,x,y,z]: [-0.4168798284 0.4702076899 0.2173425926 -0.7469124008 ]

This result is obviously unreasonable, and I found that the final ground truth values provided in the code also seems incorrect.
// Define final ground truth values
Eigen::Vector3d true_p_wi(-20946.817372738657, -3518.039994126535, 8631.1520460773336);
Eigen::Vector3d true_v_wi(15.924719563070044, -20.483884216740151, 11.455154466026718);
Eigen::Quaterniond true_q_wi(0.98996033625708202, 0.048830414166879263, -0.02917972697860232, -0.12939345742158029);

[dmkr001]

Can you help me see where the problem is？Thanks in advance.

[Chris-Bee]

Hello @dmkr001,

Thank you for the feedback. You are right, the headers are missing for this particular example, I will add this shortly. As for the final result, the true values originate from another example and are invalid for the provided THL sim data. Appologies for that, I will remove that section.

Despite the incorrect comparison of the result and the ground truth, the result you posted seems incorrect anyway. According to the final result and the measurement of the angular velocity
w_m: [ 4.2713690019 2.0292181981 8.5375555121 ]
it almost looks like you might have placed the header for w_m above the columns of the linear acceleration.

As a short fix on your side, you can use the header described for the core states here, copy the header of the CSV file from 'source/tests/test_data/traj_test_dummy.csv' or use this header:
t, w_x, w_y, w_z, a_x, a_y, a_z, p_x, p_y, p_z, v_x, v_y, v_z, q_w, q_x, q_y, q_z, bw_x, bw_y, bw_z, ba_x, ba_y, ba_z

Please re-run the scenario and let me know if this works for you.

[dmkr001]

Hello @Chris-Bee,
Thanks for your reply. I followed your advice and got the results below:
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.0047425855 0.0059544591 0.2402648105 ]
v_wi: [ -0.0138561609 0.0135512500 -0.0005736589 ]
q_wi: [ 0.9662510775 0.1197939813 -0.2225055263 0.0499954793 ]
b_w: [ -0.0021034077 0.0022079408 0.0019894341 ]
b_a: [ -0.0074781321 -0.0058735316 0.0019363983 ]
w_m: [ -1.2525263620 -0.2494051525 -1.5072852095 ]
a_m: [ 4.2713690019 2.0292181981 8.5375555121 ]
Are the results correct now?

[Chris-Bee]

The final values of this trajectory described by source/examples/mars_thl/sim_data/thl/traj.csv are:

p_wi: [8.5658567300612559e-05, 4.3444600508988977e-05, 0.23135214929044903]
v_wi: [9.5388159585275733e-07, 4.8379287855651084e-07, 0.0025763045577999371]
q_wi: [0.96669033669572046, 0.11997438321664472, -0.21914003814072092, 0.055619996355481538]

This matches your result closely. However, I noticed that you are initializing the filter to p_wi: [0 0 5] which was default. However, this data set starts at [0 0 0], if you look at error plots, you should see that there is an error in position at the beginning , which converges towards the end.

Let me know if this solves your problem.

[dmkr001]

Thanks for your answer. Your advice is helpful and solved my problem. But I faced another problem. I tried to use the GPS data provided in the example instead of the position one (for now is imu + GPS). However, it got the error result as below:
Warning: [CoreLogic: Core cov prior]: The covariance matrix is not symmetric
Warning: Eigenvalue decomposition has imaginary components
Warning: The error angle vector is not small
Warning: The error angle vector is not small
Last State:
p_wi: [ nan nan nan ]
v_wi: [ -nan nan nan ]
q_wi: [ nan nan nan nan ]
b_w: [ nan -nan -nan ]
b_a: [ nan nan -nan ]
w_m: [ -1.2525263620 -0.2494051525 -1.5072852095 ]
a_m: [ 4.2713690019 2.0292181981 8.5375555121 ]

I just followed the usage of the position sensor in the example.
Eigen::Matrix<double, 3, 1> gps_meas_std;
gps_meas_std << 0.8545, 0.8545, 2.126;
gps_sensor_sptr->R_ = gps_meas_std.cwiseProduct(gps_meas_std);

mars::GpsSensorData gps_init_cal;
Eigen::Matrix<double, 3, 1> gps_std;
gps_std << 0.8545 * 5, 0.8545 * 5, 2.126 * 5;
gps_init_cal.sensor_cov_ = gps_std.cwiseProduct(gps_std).asDiagonal();

Could you have a look on that? Thanks in advance.

[Chris-Bee]

@dmkr001 Initially you used a pose sensor which provides 6 DoF. If you switch to the GPS data, you only get 3DoF position information. You need to pay attention to how the orientation is initialized, e.g. by using the magnetometer data.

In addition, 'gps_position_1.csv' provides you with ENU position data, not GPS coordinates. I see that the labeling is misleading. I assume that you are using the GPS sensor class for 'gps_sensor_sptr', you need to change the sensor class and data type to the position sensor. The rest of the sensor definition looks correct.

Also, because I see the NAN's in the result, these files also need a CSV header. I will push a correction for this next.

I hope this helps.

[Chris-Bee]

I added the fix for CSV headers here: 47a1025

[Chris-Bee]

@dmkr001 I hope this solved your issue. I'll close this issue but we can re-open it if the problem persist.