Closed SteveMacenski closed 5 years ago
SteveMacenski
commented
6 years ago For context, this came up when fusing in a couple of sensors with tricky and changing biases I'm still not 100% catching, so when standing still for long periods I see a little drift that I hadn't seen beforehand.
ayrton04
commented
6 years ago So just backing up, the prediction step predicts the next covariance as P_pred = JPJ' + Q. If the robot isn’t moving at all, we zero out Q via the method you linked. This prevents the state estimate covariance P from growing when we’re stationary. If the robot is moving and then stops, it doesn’t make sense for its uncertainty to grow once it’s at rest.
As far as scaling with velocity, the process noise covariance can be used to model how well or poorly the kinematic model I’m using matches your robot’s kinematics. For example, if you zero out all the 3D variables and Y variables, you end up with the unicycle model. If you were using this to track the state of, say, an Ackermann vehicle, then the process noise could represent the mismatch in the vehicle’s true kinematics with the unicycle model.
In that case, if the vehicle made a very small motion (low velocity) over some time delta dt, the amount of process noise would be small, as our state prediction wouldn’t differ from reality that strongly. If it moved a larger distance (high velocity) over that same dt, then our predicted state should have more error, because it differs more from the robot's true state.
since there are less ticks per rotation (assuming an encoder is involved)
Did you mean fewer ticks per second? I would think the encoder would have a constant number of ticks per rotation. I would also think that the maximum amount of error our state estimate could accrue between two wheel encoder ticks (in this case, wheel odometry measurements in a nav_msgs/Odometry message) would be constant, regardless of the velocity.
Can you give me a bit more detail about your issue? What was the behavior beforehand? I'm assuming the drift is from integrating your biases, in which case you may not want to use dynamic process noise at all (it's a parameter). That, or you can make sure your drift is captured in the covariances of your sensor measurements. That will cause the state estimate covariance to continue to grow as well, since we integrate acceleration and velocity errors.
SteveMacenski
commented
6 years ago Thanks for the detailed response - I appreciate it, helpful context for what I was proposing for other use cases. I was thinking of this in a different way but your explanation clarifies the motive. I think I got my wires crossed thinking it was scaling the sensor covariances rather than process Q.
I did mean ticks per second, thanks for catching that. I would rue the day when someone made a variable resolution encoder.
The drift is from the biases of a couple of new toys I'm playing with and also the usual suspects from accelerometers. I do a pretty decent job at dynamically estimating biases and removing them, but as the environment changes or robot moves, they tend to change and it takes a few iterations to catch up (metal shelving, metal carts, very unmetal music if I have to be on-site). That integrated over staying still for a bit can be a little annoying.
I agree now that this is not the right feature for dealing with this. Now that I sit here and say "ok, so how could I do something similar framed in a generalized enough way that its useful to the general community." Thoughts below:
Might end up that this is just now a "me" problem and I'll handle upstream of this package but I always like when I can integrate back into the larger ecosystem package.
ayrton04
commented
5 years ago So I would gladly welcome a PR that implemented any of the above. I should just continue to warn users, though, that we are (as time allows) actively developing the successor to this package. Having said that, it may be some time before I can exactly replicate the behavior of the EKF/UKF in the new package, and I'm certainly not against new features.
SteveMacenski
commented
5 years ago I saw the Fuse package and briefly went through it. I see it's implemented not far off from how Cartographer wraps up Ceres which I thought was interesting. The design intent to cover EKF/UKFs is still a little unclear to me but I'm familiar with how I've used those tools for SLAM pose-graph optimization and at the end of the day this is all just an optimization problem.
Any preference of the recommended approaches above? Bullet 1 seems to user-land specific as not everyone is working with wheeled robots to be able to assume 1 measurement source should be trusted as "stopped". A toggle service would be my preference as it makes no use-case assumptions and it only implements a feature user-land code would need to interface with (and requires alot less rigor than scaling covariance matrices along with appropriate models).
ayrton04
commented
5 years ago I'm assuming you are OK with me closing this. Feel free to reopen if you feel this ticket isn't addressed by your changes.
When the robot is not moving the dynamic scaling seems to zero out the covariance (the diagonal of
Pin the similarity transform and its transpose is0).When this is 0, that represents both a symmetric matrix as well as represent extremely high confidence in our measurements? I believe the intended application for this when the norm of the velocity vector is zero or near zero is to apply a high covariance to the process rather than a lower one.
On thinking about it, for the general case, I would think the application would want to do the opposite scaling - rather than making the process covariance low when moving slow, we'd want it to be high since there are less ticks per rotation (assuming an encoder is involved) and therefore the expected inputs degrade, which is generally true of most non-inertial sensors.
https://github.com/cra-ros-pkg/robot_localization/blob/f0b540bbd0d645c31872aabbf1ced3c2491c9dbb/src/filter_base.cpp#L132
Either way, making a case here when the velocity norm is near zero to make the covariance high to stop motion would be good. I'm willing to work on any and all of these.
Thoughts?