High fluctuation after initialization because of old state inputs

[otim]

When the pose_msf is running along with an externally propagated state like on the asctec fcu, a problem arises when we initialize the filter. It seems that messages from the topic hl_state_input aka ekf_state_out are arriving after the msf has been initialized which still contain an old state. This state can consist of arbitrary values since it has probably only integrated imu measurements. As soon as the msf has been initialized it tries to correct the arriving states which leads to high fluctuation of the state estimate.

The following output shows the problem. I simply print the position directly in StateCallback

p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
[ INFO] [1424870597.619524987]: initial measurement pos:[  0.00162001 -0.000360376   0.00339364] orientation: [0.603, -0.000112, -0.798, 0.00252]
[ WARN] [1424870597.734423432]: Using simulated core plus fixed diag initial error state covariance.
[ WARN] [1424870597.740409060]: imu time is off by 24.794 ms
[ INFO] [1424870597.744663346]: Initializing msf_core (built: Feb 25 2015)
[ INFO] [1424870597.744723675]: Core parameters: 
fixed_bias: 0
fuzzythres: 0.1
noise_acc:  0.083
noise_accbias:  5e-05
noise_gyr:  0.0013
noise_gyrbias:  1.3e-06

[ INFO] [1424870597.744866759]: Core init with state: 
--------- State at time 597.634s: ---------
0 : [0-2]    : Matrix<3, 1>         : [ 0.0984355 0.00263127  0.0588503]
1 : [3-5]    : Matrix<3, 1>         : [0 0 0]
2 : [6-9]    : Quaternion (w,x,y,z) : [0.00593, 0.0152, -0.000825, -1]
3 : [10-12]  : Matrix<3, 1>         : [0 0 0]
4 : [13-15]  : Matrix<3, 1>         : [0 0 0]
5 : [16-16]  : Matrix<1, 1>         : [1]
6 : [17-20]  : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]  : Matrix<3, 1>         : [0 0 0]
8 : [24-27]  : Quaternion (w,x,y,z) : [0.00171, 0.788, -0.0043, 0.615]
9 : [28-30]  : Matrix<3, 1>         : [ 0.09848 0.001745 -0.05249]
-------------------------------------------------------
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb = -36.2338  3.88895 -6.06195
p in fcu state out cb =  0.0984355 0.00263127  0.0588503
p in fcu state out cb =  0.0984318 0.00263357  0.0588495
p in fcu state out cb =  0.0984185 0.00264162  0.0588469

The correction sent to via correction aka ekf_state_in would then contain something like [36.1... , -3.88..., 6.01...]