android 陀螺仪滤波_android – 卡尔曼滤波器 – 指南针和陀螺仪

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

我正在将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.su