Kalman Filter - Compass and Gyro

Question

I'm trying to build compass with gyro, accelerometer and magnometer.

I'm fusing the acc values with the magnometer values to get the orientation (using the rotation matrix) and it's working pretty well.

But now I want to add gyroscope to help compensate when the magnetic sensor is not accurate. So I want to use kalman filter to fuse the two results and get a nice filtered one (the acc and mag are already being filtered using lpf).

My matrices are:

 state(Xk) => {Compass Heading, Rate from the gyro in that axis}.
 transition(Fk) => {{1,dt},{0,1}}
 measurement(Zk) => {Compass Heading, Rate from the gyro in that axis}
 Hk => {{1,0},{0,1}}
 Qk = > {0,0},{0,0}
 Rk => {e^2(compass),0},{0,e^2(gyro)}

And this is my Kalman filter implementation:

public class KalmanFilter {

private Matrix x,F,Q,P,H,K,R;
private Matrix y,s;

public KalmanFilter(){
}

public void setInitialState(Matrix _x, Matrix _p){
    this.x = _x;
    this.P = _p;
}

public void update(Matrix z){
    try {
        y = MatrixMath.subtract(z, MatrixMath.multiply(H, x));
        s = MatrixMath.add(MatrixMath.multiply(MatrixMath.multiply(H, P), 
                        MatrixMath.transpose(H)), R);
        K = MatrixMath.multiply(MatrixMath.multiply(P, H), MatrixMath.inverse(s));
        x = MatrixMath.add(x, MatrixMath.multiply(K, y));
        P = MatrixMath.subtract(P, 
                        MatrixMath.multiply(MatrixMath.multiply(K, H), P));
    } catch (IllegalDimensionException e) {
        e.printStackTrace();
    } catch (NoSquareException e) {
        e.printStackTrace();
    }
    predict();
}

private void predict(){
    try {
        x = MatrixMath.multiply(F, x);
        P = MatrixMath.add(Q, MatrixMath.multiply(MatrixMath.multiply(F, P), 
                        MatrixMath.transpose(F)));
    } catch (IllegalDimensionException e) {
        e.printStackTrace();
    }
}

public Matrix getStateMatirx(){
    return x;
}

public Matrix getCovarianceMatrix(){
    return P;
}

public void setMeasurementMatrix(Matrix h){
    this.H = h;
}

public void setProcessNoiseMatrix(Matrix q){
    this.Q = q;
}

public void setMeasurementNoiseMatrix(Matrix r){
    this.R = r;
}

public void setTransformationMatrix(Matrix f){
    this.F = f;
}
}

At first this start values are given:

 Xk => {0,0}
 Pk => {1000,0},{0,1000}

Then I watch for the two results (the kalman one and the compass one). The kalman one is starting from 0 and increasing at some rate regardless to the measured one(compass) and it wont stop just keep increasing...

I can't understand what I'm doing wrong?

Answer 1

The problem you're seeing is that while the gyros have very low noise, it is not zero mean. When you use your term e^2(gyro) you are implementing a filter where you are claiming that z_gyro = true_gyro + v where v ~ N(0, e^2) The truth is more like v ~ N(bias, e^2) where even the bias has a few terms (primarily static turn-on bias plus a bias shift caused by temperature drift). As a result, you are integrating the bias and rotating constantly.

If you calibrate out that bias (just measure the output of the gyro when stationary) then you could call update(imu - bias) instead of just update(imu). You might have to increase e^2(gyro) to account for the shifts in bias, but not as much as if you tried to account for all of it (the uncompensated offset will turn into a fixed heading displacement in proportion to the R terms of the magnetometer and gyro).

The best way is to add the bias to your state vector. You get something like Hk = {{1,0,0},{0,1,1}}, meaning your predicted gyro measurement is the true rate plus your bias term. The magic of the Kalman Filter here is that even though you've said your measurement is simply the sum of two terms, they are different in several key ways:

• In F the heading is related to the actual turn rate (by dt), and thus the state covariance P evolves off-diagonal terms relating the heading and the turn rate each time you update P.
• Similarly in H you have described a relationship between bias and gyro rate that expresses the idea "either I'm turning faster or I have more bias" so the filter updates the state to balance those two possibilities based on the noise covariances.
• In Q, the turn rate process noise has to be set rather high to account for whatever unexpected movement you are measuring. But the Q for bias is much, much smaller because bias is not evolving very rapidly (in fact, the best model is probably a first order Gauss-Markov process, which I will not explain here, other than to throw out another useful Google term in "limited memory filter"). In the limit, you could imagine the Q term for the bias to be 0 (modelling bias as a random constant) but that doesn't work well numerically in an EKF, and isn't strictly true due to bias drift.
• Similarly, the initial P_0 of the system is much smaller for the bias term (its total possible range being documented in the datasheet) than for the totally unknown heading/angular velocity.
• In a multi-axis system the bias always goes with the axis (it's a property of the hardware unrelated to how it is oriented) but the influence of the gyro on a state like "heading" is being rotated around because of the strap-down IMU.

Watching an EKF "learn" a value like the gyro bias is even more magical to me than the prediction of the rest of the state.