Extended Kalman filter converging to incorrect value, is this just the nature of the beast? (pics)

Question

I've designed a quaternion extended Kalman filter for fusing gyroscope and accelerometer data. The shape of the estimate plot seems perfect, however the estimate seems to be constantly converging to the wrong solution. Is this just down to the fact that I'm not using an optimal estimator like the linear Kalman filter, or should it be possible to obtain an unbiased estimate using the EKF? I've used two different implementations so far and ran into the same problem both times.

Here is a plot of the filter output:

• Green: angle estimated from accelerometer alone
• Blue: Integrated gyroscope output
• Red: Linear KF output
• Cyan: EKF output, note the offset

Here is the matlab code for one iteration:

function [ q, wb ] = EKF2( a,w,dt )


persistent x P;

% Tuning paramaters
Q = [0, 0, 0, 0, 0, 0, 0;
    0, 0, 0, 0, 0, 0, 0;
    0, 0, 0, 0, 0, 0, 0;
    0, 0, 0, 0, 0, 0, 0;
    0, 0, 0, 0, 0.01, 0, 0;
    0, 0, 0, 0, 0, 0.01, 0;
    0, 0, 0, 0, 0, 0, 0.01];

R = [1000000,    0,    0;
        0, 1000000,    0;
        0,    0, 1000000;];

if isempty(P)    
    P = eye(length(Q))*10000; %Large uncertainty of initial values
    x = [1, 0, 0, 0, 0, 0, 0]';
end

q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);
wxb = x(5);
wyb = x(6);
wzb = x(7);
wx = w(1);
wy = w(2);
wz = w(3);

z(1) = a(1);
z(2) = a(2);
z(3) = a(3);
z=z';

% Populate F jacobian
F = [              1,  (dt/2)*(wx-wxb),  (dt/2)*(wy-wyb),  (dt/2)*(wz-wzb), -(dt/2)*q1, -(dt/2)*q2, -(dt/2)*q3;
    -(dt/2)*(wx-wxb),                1,  (dt/2)*(wz-wzb), -(dt/2)*(wy-wyb),  (dt/2)*q0,  (dt/2)*q3, -(dt/2)*q2;
    -(dt/2)*(wy-wyb), -(dt/2)*(wz-wzb),                1,  (dt/2)*(wx-wxb), -(dt/2)*q3,  (dt/2)*q0,  (dt/2)*q1;
    -(dt/2)*(wz-wzb),  (dt/2)*(wy-wyb), -(dt/2)*(wx-wxb),                1,  (dt/2)*q2, -(dt/2)*q1,  (dt/2)*q0;
                   0,                0,                0,                0,          1,          0,          0;
                   0,                0,                0,                0,          0,          1,          0;
                   0,                0,                0,                0,          0,          0,          1;];

%%%%%%%%% PREDICT %%%%%%%%%
%Predicted state estimate
% x = f(x,u)
x = [q0 - (dt/2) * (-q1*(wx-wxb) - q2*(wy-wyb) - q3*(wz-wzb));
     q1 - (dt/2) * ( q0*(wx-wxb) + q3*(wy-wyb) - q2*(wx-wzb));
     q2 - (dt/2) * (-q3*(wx-wxb) + q0*(wy-wyb) + q1*(wz-wzb));
     q3 - (dt/2) * ( q2*(wx-wxb) - q1*(wy-wyb) + q0*(wz-wzb));
     wxb;
     wyb;
     wzb;];

% Re-normalize Quaternion
qnorm = sqrt(x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2);
x(1) = x(1)/qnorm;
x(2) = x(2)/qnorm;
x(3) = x(3)/qnorm;
x(4) = x(4)/qnorm;

q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);

% Predicted covariance estimate
P = F*P*F' + Q;


%%%%%%%%%% UPDATE %%%%%%%%%%%
% Normalize Acc and Mag measurements
acc_norm = sqrt(z(1)^2 + z(2)^2 + z(3)^2);
z(1) = z(1)/acc_norm;
z(2) = z(2)/acc_norm;
z(3) = z(3)/acc_norm;

h = [-2*(q1*q3 - q0*q2);
     -2*(q2*q3 + q0*q1);
     -q0^2 + q1^2 + q2^2 - q3^2;];

%Measurement residual
% y = z - h(x), where h(x) is the matrix that maps the state onto the measurement
y = z - h;


% The H matrix maps the measurement to the states
H = [ 2*q2, -2*q3,  2*q0, -2*q1, 0, 0, 0;
     -2*q1, -2*q0, -2*q3, -2*q2, 0, 0, 0;
     -2*q0,  2*q1,  2*q2, -2*q3, 0, 0, 0;];

% Measurement covariance update
S = H*P*H' + R;

% Calculate Kalman gain
K = P*H'/S;

% Corrected model prediction
x = x + K*y;      % Output state vector

% Re-normalize Quaternion
qnorm = sqrt(x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2);
x(1) = x(1)/qnorm;
x(2) = x(2)/qnorm;
x(3) = x(3)/qnorm;
x(4) = x(4)/qnorm;


% Update state covariance with new knowledge
I = eye(length(P));
P = (I - K*H)*P;  % Output state covariance

q = [x(1), x(2), x(3), x(4)];
wb = [x(5), x(6), x(7)];

end

Answer 1

I have been contacted by some people having similar issues as the one stated in the first post. I would like to include a Github repository of mine where you will find a complete library to estimate the orientation using a MIMU.

The methods included are: Acceleration and magnetic field projections (this one is just to show why fusion is necessary), Regular Kalman Filter, a Extended Kalman Filter, Gated Kalman Filter and a Gated Extended Kalman Filter.

You will also find a paper explaining the process and the slides I used in the conference where I presented the paper.

Everything is commented thoroughly so I hope you are able to understand what is done in each step.

Let me know if you have any doubts about it.

https://github.com/aolivares/inmed2014

Cheers!

Answer 2

Currently I am testing both the EKF and the Complementary Filter performances to estimate the attitude with sensor fusion (gyro, acc, mag) in the quaternion form. The ultimate scope is to get different velocity estimates. At the moment I have a Nonlinear Complementary Filter that does a good job to estimate the attitude, then I subtract the gravity and integrate to get the velocity. It works fine on the short period.

Take this code as mine and consider that the measured output of my EKF are the accelerometer readings where the inertial acceleration is NOT negligible. Hence, my predicted output matrix "h" the result of z = T*acc, where T is the transformation matrix from sensor to local frame and acc are the accelerometer readings affected by the gravity. Assume there are almost no yaw rotations, but just significant partial rotations (<90deg) around pitch and roll.

The thing I cannot explain by myself is that EKF performs better if I consider negligible the inertial acceleration than when I consider that significant inertial accelerations are measured. Due to the movement I want to observe, I need to consider inertial accelerations though.

Answer 3

I have just tried you algorithm and it works really well. In order to avoid the output of the filter to follow the drifting integrated angular rate you need to decrease the value of the variance of the measurement noise. I have tried with a value of 100 instead of 1000000. This way you tell the filter to 'trust' more the observation and, therefore, the angle estimate does not diverge.

I have also implemented a script which optimizes the fix parameters (variances of the process noise and measurement noises) using an external angle reference. This way it is easier to bound their value.

It is also important to notice that the value of R and Q will depend on the intensity of the movement, i.e. the amount of linear acceleration 'corrupting' the accelerometer estimate. To increase the precision of the estimation I have implemented an intensity detector which varies for each sample the value of R and Q depending on the intensity of the motion. If you are interested, please contact me on: alberto.olivares (gmail.com) and I will happily send it to you (or any other person who is interested).

Answer 4

The problem you are describing is said to be one of the main drawbacks of the EKF. It does not guarantee any convergence to the real value. If you want to keep using it:

• Try increasing the system noise Q and/or measurement noise R (can be interpreted as 'putting the nonlinearities into the noise'). This also makes the linear KF perform better on nonlinear problems
• To judge how well you're doing, plot the 2-sigma-band around your estimate to see if your real value lies within in it (else you noise is too small).

The Unscented Kalman filter has the reputation of being more robust in dealing with non-linearities than the EKF. The implementation complexity is roughly the same as EKF.

Answer 5

I am not an expert on Kalman Filtering, but I think a static error is the best you can get with gyro/accel measurements. In my previous lab, they fused the inertial meas with GPS to recalibrate.