bug: issue when initializing with initial state

[mascheiber]

Summary

Hey @Chris-Bee, as discussed when initializing, the ros wrappers do not wait for the initial states to be set through the first sensor measurement, although they already include a code to set the inital states p_wi_init_ and q_wi_init_:

mars_ros/src/mars_wrapper_pose.cpp

Lines 260 to 267 in 1cf5e93

	if (!do_state_init_)
	{
	mars::PoseSensorStateType pose_cal = sensor_sptr->get_state(pose1_sensor_sptr_->initial_calib_);
	q_wi_init_ = pose_meas.orientation_ * pose_cal.q_ip_.inverse();
	p_wi_init_ = pose_meas.position_ + q_wi_init_.toRotationMatrix() * (-pose_cal.p_ip_);
	do_state_init_ = true;
	}

Detailed Information

The following core init call does not wait for the do_state_init_ to be set to true

mars_ros/src/mars_wrapper_pose.cpp

Line 170 in dacc3c5

if (!core_logic_.core_is_initialized_)

Instead, the line should look like

-  if (!core_logic_.core_is_initialized_ )
+  if (!core_logic_.core_is_initialized_ && do_state_init_)

This can yield the issue of the first couple of published states being wrong due to the filter converging after initializing its core state to identity (0 in position, and unit quaternion in orientation).

See graph for detailed example

The red vertical bar indicates when the initialization was called.

This issue can also apply to other wrappers and is especially present if the pose sensor provides updates significantly different from the identity/origin.