Filtered odometry drifts when no measurements received

Question

I'm trying to fuse wheel odometry with visual poses computed from observed visual markers. Whenever the robot is standing still (no odometry received) and there's no visual marker in sight (no visual pose as well), the filtered estimation starts drifting with increasing velocity.

First I thought this is caused by nonzero velocity remaining in the "twist" part of the odometry message. But it also occurs if I use absolute x, y and yaw measurements, like in the following launch file:

<launch>

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="10"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <param name="world_frame" value="odom"/>

      <param name="odom0" value="odometryPose"/>
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="pose0" value="visualPose"/>
      <rosparam param="pose0_config">[true, true, false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

    </node>

</launch>

Sure, when there's no measurement (not even an "I'm not moving!"), the uncertainty grows. Thus, I shouldn't believe in the computed expected pose too much.

But, is there any configuration I'm missing? Like don't filter without any new measurement? I'd like to avoid introducing another dummy node publishing pseudo zero measurements in case of stillstand.

Update 1

I noticed that the filtered pose jumps back to where it should be, as soon as a new measurement arrives. So the drift only seems to affect the prediction and vanishes with the next correction step. But how can a node - receiving the /odometry/filtered message - distinguish between a vague prediction and a corrected pose based on measurements?

Update 2

After apparently having fixed the issue, there's a drift again. This time I boiled it down to the following minimum example:

<launch>                                                                                  

    <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization_ekf" clear_params="true">
    
      <param name="two_d_mode" value="true"/>                                             
      <param name="world_frame" value="odom"/>                                            
    
      <param name="odom0" value="wheelOdometry"/>                                         
      <rosparam param="odom0_config">[false, false, false,                                
                                      false, false, false,                                
                                      true, true, false,                                  
                                      false, false, true,                                 
                                      false, false, false]</rosparam>                     
        
    </node> 
        
    <node pkg="unit_tests" type="publishers.py" name="publishers" />                      
        
</launch>

The launch script runs robot_localization and the following message publisher:

rospy.init_node('publishers')
wheelPub = rospy.Publisher("/wheelOdometry", Odometry, queue_size=1)
while not rospy.is_shutdown():
    wheelPub.publish(Odometry(header=Header(stamp=rospy.Time.now(), frame_id="odom"), child_frame_id="base_link"))
    rospy.Rate(10).sleep()

In this example, there is no drift. But the covariance of /odometry/filtered is growing unboundedly which indirectly causes a drift when adding a second data source like visual pose estimations.

So how can the uncertainty increase if the only information given is "My velocity is zero. I'm 100 % sure."?

---

Originally posted by Falko on ROS Answers with karma: 268 on 2015-10-19

Post score: 0

---

Original comments

Comment by Tom Moore on 2015-10-22:
Can you post some sample messages? Also, are you saying that when your robot stands still, your odometryPose message stops being published?

Comment by Falko on 2015-11-16:
@tom-moore: Sorry to bother you again. But after isolating the issue in a unit test I'm struggling with understanding the resulting odometry. See Update 2 for a complete launch file and rospy node.

Answer 1

The EKF follows a predict/update cycle. If no measurement is received within the sensor_timeout period, the filter will carry out a prediction step. If no sensor is available to correct it, the error will grow rapidly, and I wouldn't be surprised to see your pose drift. Is there a reason why your wheel odometry can't publish messages when standing still? I would make it publish messages with decreased covariance values, as you can be certain the robot isn't moving. Also, I'd switch your odometry source so that it is fusing velocity and not pose data.

Response to Update 2

If you just fuse velocities, there will always be unbounded error growth in your pose. You have nothing correcting the covariance for your pose variables. At every time step, we do a prediction, which causes the errors to grow for every variable. Then we immediately correct with a measurement, and any variable that is measured sees its covariance (usually) decrease in the state estimate. If your variable is never measured, it will never be corrected. If you watch the covariance on your twist data, you'll see that it's bounded.

I'm not sure I understand why adding pose input would result in drift. If anything, it should stick very close to your pose estimate, with some slight corrections as you move around.

---

Originally posted by Tom Moore with karma: 13689 on 2015-11-10

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Falko on 2015-11-12:
Thanks, Tom! After reviewing/refactoring the whole setup the drift vanished and I couldn't reproduce the drift. (That's why I struggled with providing samples.) Basically, I followed your suggestions: Continue to send odometry with small variance when standing still + fuse velocity rather than pose

Comment by Falko on 2015-11-25:
Ok. I thought pose (position) and twist (velocities) are somehow coupled. If I have an old pose and new zero-twist measurements with zero uncertainty - how should the pose uncertainty grow. But maybe this is a technical detail of your model and/or implementation. I'll try to find a workaraound.

Comment by Tom Moore on 2015-11-28:
They are coupled in the sense that position = velocity * time, but that relationship is only exercised during prediction. Also, you can't have zero values in your covariance matrix diagonal. That matrix needs to be invertible.

Answer 2

Based on your experience in your updated answer I would intuitively say that the noise parameter of your odometry is off. So you would need to tune your Kalman filter. The tuning parameters of a Kalman filter are process and observation noise levels (assuming your model is correct and can be simplified well enough as having gaussian noise).

Could you try and increase the process noise level of the odometry? Because you don't want the filter to drift between measurement updates (of your visual pose), it should always yield the best possible estimate. I don't know the package, so I can't tell you exactly where to change the parameters.

Remember: tuning a kalman filter can be tedious..

---

Originally posted by nyquist09 with karma: 191 on 2015-10-21

This answer was NOT ACCEPTED on the original site

Post score: 2