I am using mars_position_node to fuse IMU readings and 3D position point and I am very curious about the strategy you use in config file for determining the core state covariance and position1_state_init_cov?

Currently I am only modifying some of the parameters according to my own data, I use EVO to compare the pose_state_out from mars to the groundtruth trajectory, but the result is worse than MSF from ethzasl_MSF(full filter). I wonder whether it is caused by my wrong config settings or the so-called "negligible losses in accuracy" in your paper(but I only use one update sensor).

Here is my position_config.yaml, basically I do not change the core state covariance:

NOTE: Do not use 1e-6 format because this is reqognized as a string

Framework Settings

pub_on_prop: true
use_ros_time_now: false
verbose: true
verbose_out_of_order: true
discard_ooo_prop_meas: false
pub_cov: true
pub_path: false
buffer_size: 2000
path_buffer_size: 10000

Ros Settings

use_tcpnodelay: true
bypass_init_service: false

pub_cb_buffer_size: 1
sub_imu_cb_buffer_size: 200
sub_sensor_cb_buffer_size: 1

Sensor Settings

gyro_rate_noise: 24.0e-4 #0.426188 # Noise STD [rad/s]
gyro_bias_noise: 2.0e-5 #0.154667
acc_noise: 16.0e-3 #0.89791 # Noise STD [m/s^2]
acc_bias_noise: 5.5e-5 #0.28402

Core state covariance p,v,q,bw,ba

core_init_cov_p: [0.5,0.5,0.5]
core_init_cov_v: [0.3,0.3,0.3]
core_init_cov_q: [0.1218,0.1218,0.1218] # 20degree
core_init_cov_bw: [0.0076,0.0076,0.0076]
core_init_cov_ba: [0.01,0.01,0.01]

Pose1 sensor settings

position1_pos_meas_noise: [0.005,0.005,0]
position1_cal_p_ip: [0.0, 0.0, 0.0]
position1_state_init_cov: [0.01,0.01,0.01]

Really looking forward to your reply:)

from mars_ros.

Hello @HouChentripleg,
Thank you for your question and the remark. In general, the initialization of the core state and the calibration state depends on how you initialize the system. The initial covariance should be set according to the variance of the data used for the state initialization.

If, for example, you are using your pose or position sensor to initialize $P_{wi}$, then, in the simple case, you would set the variance of core_init_cov_p to the value of the variance of the sensor. You can do the same for the initial variance of the orientation core_init_cov_q. Also, if you know that you are not moving on initialization, then the initial covariance for the velocity core_init_cov_v can be set to a rather low value. For the IMU bias, it's slightly more empirical, either you know the uncertainty of the initialization or, if not, you can set it to a slightly higher standard deviation to allow for a more loose convergence. For the calibration of the sensor position1_cal_p_ip, also here, it depends on the accuracy of your initial value. If you are certain on the method that you used for the calibration, then the initial covariance should be accordingly.

As for the inaccuracy when comparing to MSF, could you please attach a plot of the error to evaluate this issue further? Is the error only higher in the transient phase, meaning the convergence of the estimates, or is the error persistent? How high is the error overall, e.g., the RMSE compared to MSF?

By default, the MaRS framework makes no prior assumption on cross-correlations of the core states. Thus, the initial covariance is a diagonal matrix. I know that for SSF, the predecessor of MSF, the initial covariance included cross-correlations taken from previous general sequences. This could explain one difference if MSF does the same. Also, MaRS would allow to set the full initial covariance matrix directly, if you want to test this.

Regarding the "negligible losses in accuracy", this aspect only appears when using multiple sensors. As you already pointed out in this context, you are using one sensor, and this should not cause any inaccuracies. MaRS was tested against SSF with no major differences in accuracy. We didn't test it against MSF, since they behave the same in general. I will wait for your response on the error plot to be able to give you more feedback.

I assume you are using the same data, same sensor calibration, same IMU intrinsics etc.

MaRS does not perform any zero velocity updates, and I don't know if MSF does. That could be a difference as well.

I would also be interested in the orientation estimate, and it's error. If you are using only a position sensor, then the orientation only becomes observable while you are moving (pseudo attitude) and will slowly diverge otherwise. This behavior should be the same between both frameworks.

I hope this gives you some pointers.

Best,
Christian

from mars_ros.