drop or reset upon large forward time jump #200
Conversation
| @@ -91,6 +91,8 @@ class ImuFilterRos | |||
| bool stateless_; | |||
| bool publish_tf_; | |||
| bool reverse_tf_; | |||
| bool refuse_large_time_update_; | |||
There was a problem hiding this comment.
This variable is never read and can be removed.
| @@ -168,6 +182,10 @@ ImuFilterRos::~ImuFilterRos() | |||
| void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw) | |||
| { | |||
| checkTimeJump(); | |||
| if (checkLargeTimeJumpForward(imu_msg_raw->header.stamp)) | |||
| { | |||
| return; // large jump don't use reading | |||
There was a problem hiding this comment.
I don't understand this comment. It should be something like "reject measurement if large time jump forward detected".
(repeated on line 253)
| bool largeJump = false; | ||
| if (constant_dt_ > 0.0) | ||
| { | ||
| return largeJump; // use constant dt |
There was a problem hiding this comment.
This can be simplified to return false;.
Also, the comment is not very helpful. It should be something like "don't reject measurement on large jump if using constant dt". But why does that make sense? Shouldn't we do the opposite (always reject if using constant_dt)?
There was a problem hiding this comment.
We will change the comment and the return statement. If dt is constant, it doesn't matter what the jump in the timestamp is, so we should use the measurement.
| (imu_stamp - last_time_).toSec()); | ||
| largeJump = true; | ||
| last_time_ = imu_stamp; // save time for next move | ||
| if (reset_large_forward_time_jump_) |
There was a problem hiding this comment.
I wonder if we shouldn't always reset. This would simplify the code a lot:
- we can remove the
reset_large_forward_time_jumpparameter - we can probably even remove the return value from this function, and the
largeJumpvariable - we can make this function very similar to
checkTimeJump(), and perhaps even merge this function into it.
Also, it sounds safer to always reset. The current implementation only rejects the first measurement after a large jump forward, but if the filter isn't reset, the next message will still have a large jump in values with a small dt, so rejecting the first measurement doesn't help at all.
There was a problem hiding this comment.
I don't think we should always reset. In our application, we experience one large time jump when our robot establishes communication. It could be very dangerous to reset the IMU suddenly. Instead, I believe it is better to ignore one measurement. Why should the next message have a large jump in values?
There was a problem hiding this comment.
Ah yes, after reading #160 again I remember the dilemma in your application. There can be two sources of large forward time jumps between two consecutive messages:
- Pause: If the connection to the IMU was lost somehow and messages were dropped for an extensive time, when the first message arrives again you get a large time jump and a large jump in values (because the IMU was moved in the meantime).
- Clock update: There is no connection error, and the IMU messages arrive continuously at the same rate, but the clock was moved forward between two messages. There's a large time jump but no large jump in values.
You're trying to deal with case 2 here, but I was thinking about case 1.
As I already wrote in #160 (comment), I think it's a bad idea to try to handle case 2 in the imu_filter_madgwick.
The source of the problem is the incorrect system clock. You are feeding bad data with incorrect time stamps (and a time jump) to all downstream software (such as imu_filter_madgwick). This will cause bad behavior in hundreds of different places, often with very subtle effects that will be a nightmare to track down.
Instead of trying to make the downstream software deal with incorrect inputs, you should fix the problem at the source and provide correct time stamps. Two wrongs don't make a right.
There are many ways you could fix the time jumps:
- Add a real time clock to the robot (they only cost a couple of bucks).
- Don't update the time after connecting to the internet.
- First connect to the internet, wait until the time is updated, then start the ROS software.
- Detect when time jumps happen and restart the ROS software then.
- ...
this PR is similar to #165
We discussed this a while ago (ref: #160).
The proposed solution adds the ability to drop/reset upon jump forward in time stamp