I want to fuse a Lidar-Odometry into my EKF where I'm already using encoder, IMU, GPS. The odometry has as output a delta pose, how to put that into the filter? How to deal with the covariances?
$\begingroup$
$\endgroup$
Add a comment
|
2 Answers
$\begingroup$
$\endgroup$
The best paper on this topic is SC-KF Mobile Robot Localization: A Stochastic Cloning-Kalman Filter for Processing Relative-State Measurements.
In the related approaches section it explains some of the other ways you can deal with it, and then proceeds to explain what it considers to be the best way. I highly recommend you read it. It is a quite important paper, though I'll summarize the different approaches.
The different methods are:
Treat your LIDAR measurements like a pseudo GPS/ absolute pose measuring device.
- At every LIDAR measurement you compute the sum of all the previous deltas to give you a position. You then use that position just as if you were to use a GPS sensor.
- The covariance is easy since it is just summing your delta pose covariances. Though it basically becomes useless since it just keeps on growing. However, you can reset it whenever you get a GPS measurement.
Convert your delta pose measurements to an average velocity.
- Given your delta pose $T_{xy}$ you know the time delta between Pose $X$ and Pose $Y$ and can convert it to velocity. The velocity should be in your state vector, and can now be updated in your Lidar measurement step.
- This is probably the most common method people use, since it is the easiest to implement, and doesn't have all the correlation problems the above approach might have.
- For covariance you just have to convert your pose covariances to velocity covariances.
Use stochastic cloning as described in the paper.
- Basically given a delta pose $T_{xy}$ you have a copy of your state vector at Pose $X$ and Pose $Y$. You can now apply the delta pose directly on your state vector since you have the expected observation(the delta between your Pose $X$ and Pose $Y$ copies), and your measurement the Lidar delta pose.
- Is the best method since it properly handles all of the correlations/covariances, and doesn't make assumptions like the average velocity approach.
- This is a rough explanation. I recommend reading the paper, for a better one.
$\begingroup$
$\endgroup$
I have had good luck with using the lidar for the first two lines, and using wheel odometry for the second pair of lines.
Lidar: t, t, f, f, f, t (position)
wheel: t, t, f, f, f, t (velocity)
This is for a robot that runs on flat ground. (2d movement)
