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_: https://github.com/aau-cns/mars_ros/blob/1cf5e93b324a47d1194d48ae2ed906db8f06f5da/src/mars_wrapper_pose.cpp#L260-L267

Detailed Information

The following core init call does not wait for the do_state_init_ to be set to true https://github.com/aau-cns/mars_ros/blob/dacc3c5994ef8fa50b2a257741919a20f1c1ffc0/src/mars_wrapper_pose.cpp#L170 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.