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

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.

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.

• This is actually the most common way to deal with biases in Kalman Filters so it is mathematically sound. Just add it as a parameter to the state vector. Whenever you do IMU based estimation you add a bias parameter to the state vector for the accelerometer and gyroscope. – edwinem Aug 24 at 19:09
• @edwinem - good to know, thanks! I had read about parameter estimation with Kalman filters before, but didn't know how common (or not) bias estimation is. – Chuck Aug 24 at 23:32
• @edwinem it can be noted that the added bias state is not controllable, so when applying something like LQR or pole placement one needs to take a few extra steps in order to obtain a state feedback control policy. – fibonatic Aug 25 at 18:06