I'm trying to finish up a localization pipeline, and the last module I need is a filtering framework for my pose estimates. While a Kalman filter is probably the most popular option, I'm using cameras for my sensing and I wouldn't be able to ensure the kinds of noise profiles KF is good for, I doubt it would work as well with suddenly appearing outliers in my poses: so I am looking for other options which can work with real time data and be immune to outliers.

One thing I came across is a Kalman filter with a threshold based update rejection, something like Mahalanobis distance: but I don't know if this is completely applicable because the localization would be performed in real time, and it's not like I have a large set of 'good poses' to start with. The end result I'm expecting is something like smoothing, but without access to a full set of values. Another option I found is from a paper that proposes a 'robust Bayesian weighted Kalman filter' that's good for real time outlier rejection, but I still need to read through it: and I don't have much experience with filtering/smoothing, so any suggestions would be very helpful, perhaps about a decent go-to mechanism for this?


2 Answers 2


It is both acceptable and standard to use camera observations with a Kalman filter if you are talking about landmark positions in pixel or real-world space. Pixel space observations are usually randomly Caushy distributed but it turns out the Gaussian Kalman filter works pretty well in this case.

The method you're describing using the Mahalonobis distance to reject updates is $\chi^2$ gating, where you use the "running" covariance of the estimated mean in order to determine if the candidate observation is not within a given confidence interval.

$$\chi^2 = (z - \hat{z})^\top S^{-1}(z - \hat{z})$$

You then substitute $\chi^2$ with your chosen confidence interval, which is usually chosen from a table or graph given the degrees of freedom of the system (rank of $S$).

For example $\chi^2_{0.01} = 16.812$ for 6 degrees of freedom, which means if $\chi^2 \ge 16.812$ then there is a $99\%$ chance it is an outlier and you should reject it. In order to do this you need to track the covariance $S$, the easiest way of which is to use a Kalman Filter.

A more simple method is to threshold on the Median Absolute Deviation metric.

The KF and MAD methods involves logging a small dataset and characterising variance.

NOTE: The downside to any rejection method is that if you start on an invalid pose and you have no mechanism by which to "reset", you will always be locked on an invalid pose. A simple mechanism to circumvent this to count how many consecutive rejections are made, and if it's above say 2 sec worth of data then force the filter to update (ignore the rejection) and reset your $P$ matrix.

  • 2
    $\begingroup$ I use, if (residual > 3* sqrt(Measurement_covariance_noise)then >>>>>reject the outlier; end if I want to try chi square test, How did you choose the threshold ? Because, if you determine the rank of measurement covariance noise, it will change in all steps. you need the all measurement for data median so that can not work online! $\endgroup$
    – Ugur
    Jul 10, 2018 at 12:20

I really like the answer of Gouda. One question though, you state "Pixel space observations are usually randomly Caushy distributed" how does this come about? Could you point me to some references or give an intuitive explanation? Thanks!

Regarding outlier robust Kalman filtering there are two similar approaches to the paper High-Voltage included. One is based on L1-regularization, see Doubly Robust Smoothing of Dynamical Processes via Outlier Sparsity Constraints, and the other is based on sparse Bayesian learning, see Outlier-insensitive Kalman Smoothing and Marginal Message Passing.

  • 2
    $\begingroup$ Welcome to Robotics Effesian. On Stack exchange answers should only answer the question. If you have a related question, it should be asked as a new question (ideally referencing this one). If you wish to discuss a question or answer you can use Robotics Chat, when you have chat privileges. If you have an idea for how a question or answer can be improved, you can comment when you have comment privileges. Please review How to Ask and tour for more information. $\endgroup$
    – Mark Booth
    May 29, 2017 at 11:37

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.