Kalman Filter - Compass and Gyroscope

Question

I am trying to build a compass with a gyroscope, accelerometer and magnetometer.

I am fusing acc values ​​with magnon values ​​to get the orientation (using the rotation matrix), and it works very well.

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

My matrices:

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 implementation of the Kalman filter:

 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; } } 

First, the initial values ​​begin:

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

Then I look at two results (one Kalman and one compass). Kalman alone starts at 0 and grows at a certain speed regardless of the measured (compass), and it does not stop, it just keeps increasing ...

I don’t understand what am I doing wrong?

Answer 1

The problem you see is that although gyroscopes have a very low noise level, this does not mean zero. When you use your term e^2(gyro) , you introduce a filter in which you state that z_gyro = true_gyro + v where v ~ N(0, e^2) truth is more like v ~ N(bias, e^2) , where even the bias has several terms (primarily, the static bias when turned on plus the bias bias caused by temperature drift). As a result, you integrate the displacement and constantly rotate.

If you calibrate this offset (just measure the output of the gyroscope when stationary), you can call update(imu - bias) instead of update(imu) . You may need to increase e^2(gyro) to account for shifts in the bias, but not as much as if you tried to take all of this into account (an uncompensated bias will turn into a fixed bias in the header proportional to the conditions of the R magnetometer and gyroscope).

The best way is to add an offset to the state vector. You get something like Hk = {{1,0,0},{0,1,1}} , which means that your predicted gyro measurement is the true rate plus your offset period. The magic of the Kalman filter here is that although you said that your dimension is just the sum of two terms, they differ in several key ways:

• In F header is related to the actual rotation speed (by dt ), and thus covariance of the state of P develops off-diagonal terms linking the header and the rotation speed each time you update P
• Similarly, in H you described the relationship between displacement and gyroscope speed, which expresses the idea of ​​“either I get faster or I have more bias,” so the filter updates the state to balance these two possibilities based on covariance noise.
• In Q noise of the rotation speed process must be set high enough to account for any unexpected movement that you are measuring. But the Q for bias is much smaller because the bias does not develop very quickly (in fact, the best model is probably the first-order Gauss-Markov process, which I will not explain here, in addition to ejecting another useful Google term " limited memory filter "). In the limit, one can imagine that the term Q for bias is 0 (modeling bias as a random constant), but this does not work very well numerically in EKF and is not strictly true due to bias bias.
• Similarly, the initial P_0 system is much smaller for the offset member (its full possible range is documented in the data table) than for the completely unknown header speed / angular.
• In a multi-axis system, the displacement always goes with the axis (this is a property of equipment not related to how it is oriented), but the influence of the gyroscope on the “heading” type rotates around because of the belt down IMU.

EKF's observation “teaches” a meaning like the displacement of gyroscopes, even more magical for me than predicting the rest of the state.