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