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

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 2

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.