小编use*_*033的帖子

我正在尝试用陀螺仪,加速度计和万用表构建罗盘.

我正在将acc值与测量值融合以获得方向(使用旋转矩阵)并且它工作得非常好.

但现在我想添加陀螺仪,以便在磁传感器不准确时进行补偿.因此,我想使用卡尔曼滤波器来融合两个结果并获得一个很好的过滤结果(acc和mag已经使用lpf进行过滤).

我的矩阵是:

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

这是我的卡尔曼滤波器实现:

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, …