------ RosFilter::imuCallback (imu0) ------
IMU message:
header:
seq: 17
stamp: 1600413727.720696466
frame_id: imu_link
orientation:
x: -0.0032349
y: -0.023407
z: 0.65857
w: 0.75214
orientation_covariance[]
orientation_covariance[0]: 100
orientation_covariance[1]: 0
orientation_covariance[2]: 0
orientation_covariance[3]: 0
orientation_covariance[4]: 100
orientation_covariance[5]: 0
orientation_covariance[6]: 0
orientation_covariance[7]: 0
orientation_covariance[8]: 100
angular_velocity:
x: 3.3569
y: 1.0376
z: 0.24414
angular_velocity_covariance[]
angular_velocity_covariance[0]: 1
angular_velocity_covariance[1]: 0
angular_velocity_covariance[2]: 0
angular_velocity_covariance[3]: 0
angular_velocity_covariance[4]: 0.0001
angular_velocity_covariance[5]: 0
angular_velocity_covariance[6]: 0
angular_velocity_covariance[7]: 0
angular_velocity_covariance[8]: 0.0001
linear_acceleration:
x: 0.13877
y: -0.41631
z: 10.279
linear_acceleration_covariance[]
linear_acceleration_covariance[0]: 0.002
linear_acceleration_covariance[1]: 0
linear_acceleration_covariance[2]: 0
linear_acceleration_covariance[3]: 0
linear_acceleration_covariance[4]: 1
linear_acceleration_covariance[5]: 0
linear_acceleration_covariance[6]: 0
linear_acceleration_covariance[7]: 0
linear_acceleration_covariance[8]: 0.002
------ RosFilter::poseCallback (imu0_pose) ------
Pose message:
header:
seq: 17
stamp: 1600413727.720696466
frame_id: imu_link
pose:
pose:
position:
x: 0
y: 0
z: 0
orientation:
x: -0.0032349
y: -0.023407
z: 0.65857
w: 0.75214
covariance[]
covariance[0]: 0
covariance[1]: 0
covariance[2]: 0
covariance[3]: 0
covariance[4]: 0
covariance[5]: 0
covariance[6]: 0
covariance[7]: 0
covariance[8]: 0
covariance[9]: 0
covariance[10]: 0
covariance[11]: 0
covariance[12]: 0
covariance[13]: 0
covariance[14]: 0
covariance[15]: 0
covariance[16]: 0
covariance[17]: 0
covariance[18]: 0
covariance[19]: 0
covariance[20]: 0
covariance[21]: 100
covariance[22]: 0
covariance[23]: 0
covariance[24]: 0
covariance[25]: 0
covariance[26]: 0
covariance[27]: 0
covariance[28]: 100
covariance[29]: 0
covariance[30]: 0
covariance[31]: 0
covariance[32]: 0
covariance[33]: 0
covariance[34]: 0
covariance[35]: 100
Update vector for imu0_pose is:
[f f f f f t f f f f f f f f f ]
------ RosFilter::preparePose (imu0_pose) ------
Final target frame for imu0_pose is base_link
After rotating into the base_link frame, covariance is
[0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 100 0 0
0 0 0 0 100 0
0 0 0 0 0 100 ]
----- /RosFilter::preparePose (imu0_pose) ------
Enqueued new measurement for imu0_pose
Last message time for imu0_pose is now 1600413727.720696466
----- /RosFilter::poseCallback (imu0_pose) ------
------ RosFilter::twistCallback (imu0_twist) ------
Twist message:
header:
seq: 17
stamp: 1600413727.720696466
frame_id: imu_link
twist:
twist:
linear:
x: 0
y: 0
z: 0
angular:
x: 3.3569
y: 1.0376
z: 0.24414
covariance[]
covariance[0]: 0
covariance[1]: 0
covariance[2]: 0
covariance[3]: 0
covariance[4]: 0
covariance[5]: 0
covariance[6]: 0
covariance[7]: 0
covariance[8]: 0
covariance[9]: 0
covariance[10]: 0
covariance[11]: 0
covariance[12]: 0
covariance[13]: 0
covariance[14]: 0
covariance[15]: 0
covariance[16]: 0
covariance[17]: 0
covariance[18]: 0
covariance[19]: 0
covariance[20]: 0
covariance[21]: 1
covariance[22]: 0
covariance[23]: 0
covariance[24]: 0
covariance[25]: 0
covariance[26]: 0
covariance[27]: 0
covariance[28]: 0.0001
covariance[29]: 0
covariance[30]: 0
covariance[31]: 0
covariance[32]: 0
covariance[33]: 0
covariance[34]: 0
covariance[35]: 0.0001
Update vector for imu0_twist is:
[f f f f f f f f f f f t f f f ]
------ RosFilter::prepareTwist (imu0_twist) ------
Original measurement as tf object:
Linear: (0 0 0)
Rotational: (3.35693359375 1.03759765625 0.244140625)
Original update vector:
[f f f f f f f f f f f t f f f ]
Original covariance matrix:
[0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 1 0 0
0 0 0 0 0.0001 0
0 0 0 0 0 0.0001 ]
imu_link->base_link transform:
Origin: (0 0 0)
Rotation (RPY): (0, -0, 0)
After applying transform to base_link, update vector is:
[f f f f f f f f f f f t f f f ]
After applying transform to base_link, measurement is:
Linear: (0 0 0)
Rotational: (3.35693359375 1.03759765625 0.244140625)
Transformed covariance is
[0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 1 0 0
0 0 0 0 0.0001 0
0 0 0 0 0 0.0001 ]
----- /RosFilter::prepareTwist (imu0_twist) ------
Enqueued new measurement for imu0_twist_twist
Last message time for imu0_twist is now 1600413727.720696466
----- /RosFilter::twistCallback (imu0_twist) ------
------ RosFilter::accelerationCallback (imu0_acceleration) ------
Twist message:
header:
seq: 17
stamp: 1600413727.720696466
frame_id: imu_link
orientation:
x: -0.0032349
y: -0.023407
z: 0.65857
w: 0.75214
orientation_covariance[]
orientation_covariance[0]: 100
orientation_covariance[1]: 0
orientation_covariance[2]: 0
orientation_covariance[3]: 0
orientation_covariance[4]: 100
orientation_covariance[5]: 0
orientation_covariance[6]: 0
orientation_covariance[7]: 0
orientation_covariance[8]: 100
angular_velocity:
x: 3.3569
y: 1.0376
z: 0.24414
angular_velocity_covariance[]
angular_velocity_covariance[0]: 1
angular_velocity_covariance[1]: 0
angular_velocity_covariance[2]: 0
angular_velocity_covariance[3]: 0
angular_velocity_covariance[4]: 0.0001
angular_velocity_covariance[5]: 0
angular_velocity_covariance[6]: 0
angular_velocity_covariance[7]: 0
angular_velocity_covariance[8]: 0.0001
linear_acceleration:
x: 0.13877
y: -0.41631
z: 10.279
linear_acceleration_covariance[]
linear_acceleration_covariance[0]: 0.002
linear_acceleration_covariance[1]: 0
linear_acceleration_covariance[2]: 0
linear_acceleration_covariance[3]: 0
linear_acceleration_covariance[4]: 1
linear_acceleration_covariance[5]: 0
linear_acceleration_covariance[6]: 0
linear_acceleration_covariance[7]: 0
linear_acceleration_covariance[8]: 0.002
Update vector for imu0_acceleration is:
imu0_acceleration------ RosFilter::prepareAcceleration (imu0_acceleration) ------
Original measurement as tf object: (0.1387695312500000222 -0.4163085937500000111 10.278515625000000711)
Original update vector:
[f f f f f f f f f f f f t f f ]
Original covariance matrix:
[0.002 0 0
0 1 0
0 0 0.002 ]
imu_link->base_link transform:
Origin: (0 0 0)
Rotation (RPY): (0, -0, 0)
After applying transform to base_link, update vector is:
[f f f f f f f f f f f f t f f ]
After applying transform to base_link, measurement is:
(0.1387695312500000222 -0.4163085937500000111 10.278515625000000711)
Transformed covariance is
[0.002 0 0
0 1 0
0 0 0.002 ]
----- /RosFilter::prepareAcceleration(imu0_acceleration) ------
Enqueued new measurement for imu0_acceleration_acceleration
Last message time for imu0_acceleration is now 1600413727.720696466
----- /RosFilter::accelerationCallback (imu0_acceleration) ------
----- /RosFilter::imuCallback (imu0) ------
------ RosFilter::integrateMeasurements ------
Integration time is 1600413727.7359337807
5 measurements in queue.
------ FilterBase::processMeasurement (odom0_pose) ------
Filter is already initialized. Carrying out predict/correct loop...
Measurement time is 1600413727.7169611454, last measurement time is 1600413727.7005372047, delta is 0.016423940658569335938
---------------------- Ekf::predict ----------------------
delta is 0.016423940658569335938
state is [0.0020127 -0.006195 0 0 0 -0.71011 0.030853 0 0 0 0 8.3697e-07 0.36915 0 0 ]
Transfer function is:
[1 0 0 0 0 0 0.012454 0.010707 0 0 0 0 0.00010227 8.7926e-05 0
0 1 0 0 0 0 -0.010707 0.012454 -0 0 0 0 -8.7926e-05 0.00010227 -0
0 0 1 0 0 0 -0 0 0.016424 0 0 0 -0 0 0.00013487
0 0 0 1 0 0 0 0 0 0.016424 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0.016424 -0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0.016424 0 0 0
0 0 0 0 0 0 1 0 0 0 0 0 0.016424 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0.016424 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0.016424
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 ]
Transfer function Jacobian is:
[1 0 0 0 0 0.0003628 0.012454 0.010707 0 0 0 0 0.00010227 8.7926e-05 0
0 1 0 -0 0 0.000422 -0.010707 0.012454 -0 0 0 0 -8.7926e-05 0.00010227 -0
0 0 1 0 -0.00055651 0 -0 0 0.016424 0 0 0 -0 0 0.00013487
0 0 0 1 1.3746e-08 0 0 0 0 0.016424 0 0 0 0 0
0 0 0 -1.3746e-08 1 0 0 0 0 0 0.016424 -0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0.016424 0 0 0
0 0 0 0 0 0 1 0 0 0 0 0 0.016424 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0.016424 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0.016424
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 ]
Process noise covariance is:
[0.05 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0.05 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0.06 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0.03 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0.03 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0.06 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0.025 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0.025 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0.04 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0.01 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0.01 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0.02 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0.01 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0.01 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.015 ]
Current state is:
[0.0020127 -0.006195 0 0 0 -0.71011 0.030853 0 0 0 0 8.3697e-07 0.36915 0 0 ]
Predicted state is:
[0.0024347 -0.0065577 0 0 0 -0.71011 0.036916 0 0 0 0 8.3697e-07 0.36915 0 0 ]
Current estimate error covariance is:
[0.16428 2.0031e-05 0 0 0 0.0029363 3.2526e-05 3.8023e-05 0 0 0 6.0106e-14 9.2112e-06 0.00012828 0
2.0031e-05 0.16424 0 0 0 0.0012041 -2.8048e-05 3.566e-05 0 0 0 7.0227e-14 -8.9265e-06 4.6705e-05 0
0 0 0.0050173 -3.6657e-14 -1.2418e-06 0 0 0 6.7085e-05 9.002e-17 -2.0678e-11 0 0 0 4.2139e-07
0 0 -3.6657e-14 0.0025084 -1.6661e-18 0 0 0 5.7515e-17 1.6799e-05 3.8398e-14 0 0 0 -4.4807e-19
0 0 -1.2418e-06 -1.6661e-18 0.0025084 0 0 0 1.1068e-15 -3.8398e-14 1.6799e-05 0 0 0 -8.6225e-18
0.0029363 0.0012041 0 0 0 0.19508 0 0 0 0 0 3.3515e-05 0 0 0
3.2526e-05 -2.8048e-05 0 0 0 0 0.0021051 0 0 0 0 0 0.00019793 0 0
3.8023e-05 3.566e-05 0 0 0 0 0 0.0021978 0 0 0 0 0 0.0013072 0
0 0 6.7085e-05 5.7515e-17 1.1068e-15 0 0 0 0.0033444 -1.225e-18 -2.114e-17 0 0 0 2.5178e-05
0 0 9.002e-17 1.6799e-05 -3.8398e-14 0 0 0 -1.225e-18 0.00083626 -6.7778e-22 0 0 0 9.5434e-21
0 0 -2.0678e-11 3.8398e-14 1.6799e-05 0 0 0 -2.114e-17 -6.7778e-22 0.00083626 0 0 0 1.6469e-19
6.0106e-14 7.0227e-14 0 0 0 3.3515e-05 0 0 0 0 0 0.0016715 0 0 0
9.2112e-06 -8.9265e-06 0 0 0 0 0.00019793 0 0 0 0 0 0.0030035 0 0
0.00012828 4.6705e-05 0 0 0 0 0 0.0013072 0 0 0 0 0 0.016276 0
0 0 4.2139e-07 -4.4807e-19 -8.6225e-18 0 0 0 2.5178e-05 9.5434e-21 1.6469e-19 0 0 0 0.0012541 ]
Predicted estimate error covariance is:
[0.16511 2.1925e-05 0 0 0 0.0030071 5.8961e-05 6.403e-05 0 0 0 1.2159e-08 1.1983e-05 0.00014371 0
2.1925e-05 0.16506 0 0 0 0.0012864 -5.079e-05 6.4227e-05 0 0 0 1.4143e-08 -1.131e-05 6.4649e-05 0
0 0 0.0060059 -7.2913e-14 -2.6379e-06 0 0 0 0.00012203 1.1137e-16 -9.3695e-09 0 0 0 1.0041e-06
0 0 -7.2913e-14 0.0030019 -1.665e-18 0 0 0 5.7488e-17 3.0534e-05 2.6932e-13 0 0 0 -4.4792e-19
0 0 -2.6379e-06 -1.665e-18 0.0030019 0 0 0 1.1063e-15 -2.6932e-13 3.0534e-05 0 0 0 -8.6198e-18
0.0030071 0.0012864 0 0 0 0.19607 0 0 0 0 0 6.0968e-05 0 0 0
5.8961e-05 -5.079e-05 0 0 0 0 0.002523 0 0 0 0 0 0.00024725 0 0
6.403e-05 6.4227e-05 0 0 0 0 0 0.0026557 0 0 0 0 0 0.0015745 0
0 0 0.00012203 5.7488e-17 1.1063e-15 0 0 0 0.0040025 -1.2248e-18 -2.1138e-17 0 0 0 4.5776e-05
0 0 1.1137e-16 3.0534e-05 -2.6932e-13 0 0 0 -1.2248e-18 0.0010005 -6.7778e-22 0 0 0 9.5434e-21
0 0 -9.3695e-09 2.6932e-13 3.0534e-05 0 0 0 -2.1138e-17 -6.7778e-22 0.0010005 0 0 0 1.6469e-19
1.2159e-08 1.4143e-08 0 0 0 6.0968e-05 0 0 0 0 0 0.002 0 0 0
1.1983e-05 -1.131e-05 0 0 0 0 0.00024725 0 0 0 0 0 0.0031677 0 0
0.00014371 6.4649e-05 0 0 0 0 0 0.0015745 0 0 0 0 0 0.01644 0
0 0 1.0041e-06 -4.4792e-19 -8.6198e-18 0 0 0 4.5776e-05 9.5434e-21 1.6469e-19 0 0 0 0.0015005 ]