2
$\begingroup$

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.

$\endgroup$
3
$\begingroup$

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.

| improve this answer | |
$\endgroup$
  • 2
    $\begingroup$ 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. $\endgroup$ – edwinem Aug 24 at 19:09
  • $\begingroup$ @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. $\endgroup$ – Chuck Aug 24 at 23:32
  • $\begingroup$ @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. $\endgroup$ – fibonatic Aug 25 at 18:06

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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