How to deal with Gaussian Errors, which don't have a mean value of 0?

Question

I'm writing a SLAM algorithm. One of the main problems is, that the landmark measurement error is not really Gaussian distributed, which means, their error doesn't have a mean value of 0.

I already tried other M-Estimators, but they all make the assumptions, that the error has a mean of 0. How do you cope with these problems? I think it is safe to say, that most measurements in real life are not gaussian distributed with a mean error of 0.

Answer 1

I don't know if this is mathematically sound or not, but it's given me good results in practice. What I've done in this situation personally, when I expect/know that there is a bias, is to include the bias as a state in my state space representation of the system.

For example, if I have a system to track position, I might write the state space models like:

$$ \left[\begin{matrix} \dot{x} \\ \ddot{x} \end{matrix}\right] = \left[\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}\right]\left[\begin{matrix} x \\ \dot{x} \end{matrix}\right] + \left[\begin{matrix} 0 \\ K \end{matrix}\right] u $$

So in this model, hypothetically, the input is maybe some motor torque through a gearbox to a wheel, so the input is a torque, some constant K converts it to an acceleration. Straightforward enough, but what if I know that I'm operating on an incline, but can't measure it? Or, if I know I'm testing on flat ground, but the event track will have an incline?

The trick here requires a bit of fundamental understanding of the differential equation - a differential equation tells you how the variable changes. So, if I expect the slope to be constant in the long-term ("long" relative to the other dynamics of the system), then I can get away with describing the change in slope as being zero! So I modify my state space representation to be:

$$ \left[\begin{matrix} \dot{x} \\ \ddot{x} \\ \frac{d}{dt}\theta \end{matrix}\right] = \left[\begin{matrix} 0 & 1 & 0\\ 0 & 0 & -mg\\ 0 & 0 & 0 \end{matrix}\right]\left[\begin{matrix} x \\ \dot{x} \\ \theta \end{matrix}\right] + \left[\begin{matrix} 0 \\ K \\ 0 \end{matrix}\right] u $$

So where previously I was assuming the acceleration is some linear function of my input (torque):

$$ \ddot{x} = Ku \\ $$

Now I'm saying that the acceleration is the torque times a constant minus the effect of the slope:

$$ \ddot{x} = Ku - mg\theta \\ $$

This is a use of the small angle approximation $\sin\left(\theta\right) \approx \theta$.

Assuming I can measure position and speed, but not slope, this leaves me with the typical state-space matrices:

$$ A = \left[\begin{matrix} 0 & 1 & 0\\ 0 & 0 & -mg\\ 0 & 0 & 0 \end{matrix}\right] $$

and

$$ C = \left[\begin{matrix} 1 \\ 1 \\ 0 \end{matrix}\right] $$

and then I can check for observability in Matlab:

>> syms m g
>> A = [0, 1, 0; 0, 0, -m*g; 0, 0, 0];
>> C = [1, 0, 0; 0, 1, 0];
>> O = [C; C*A; C*A*A];
>> rank(O)

ans =

     3

And so you can see that the bias-estimating state-space form is observable, meaning that it is possible to estimate the bias term (slope, in this example) just by measuring the position and speed of the vehicle.