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

}

}

首先给出这个起始值：

Xk => {0,0}

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

然后我观察两个结果(卡尔曼一个和罗盘一个).卡尔曼一个从0开始并且以某种速率增加而不管测量的那个(罗盘)它不会停止只是继续增加……

我无法理解我做错了什么？

解决方法:

您所看到的问题是,虽然陀螺仪的噪音非常低,但它并非零均值.当你使用你的术语e ^ 2(陀螺仪)时,你正在实现一个过滤器,你声称z_gyro = true_gyro v其中v~N(0,e ^ 2)事实更像是v~N(偏差,e ^ 2即使偏置有几个术语(主要是静态导通偏置加上温度漂移引起的偏移).因此,您正在整合偏差并不断旋转.

如果你校准出那个偏差(只是在静止时测量陀螺仪的输出),那么你可以调用更新(imu – bias)而不仅仅是更新(imu).您可能必须增加e ^ 2(陀螺仪)以考虑偏差的变化,但不会像您尝试考虑所有偏差那样多(未补偿的偏移将变为与R项成比例的固定航向位移)磁力计和陀螺仪).

最好的方法是将偏差添加到状态向量中.你会得到类似Hk = {{1,0,0},{0,1,1}}的东西,这意味着你预测的陀螺仪测量值是真实的速率加上你的偏差项.卡尔曼滤波器的神奇之处在于,尽管您已经说过您的测量只是两个项的总和,但它们在几个关键方面有所不同：

>在F中,航向与实际转弯率(通过dt)相关,因此状态协方差P在每次更新P时演变与航向和转弯率相关的非对角线项.

>类似地,在H中,您已经描述了偏置和陀螺仪速率之间的关系,表达了“我转得更快或者我有更多偏差”的想法,因此滤波器根据噪声协方差更新状态以平衡这两种可能性.

>在Q中,转弯率过程噪声必须设置得相当高,以适应您测量的任何意外运动.但偏差的Q值要小得多,因为偏差不是很快发展的(实际上,最好的模型可能是一阶高斯 – 马尔可夫过程,我不会在这里解释,除了抛出另一个有用的谷歌术语在“有限的记忆过滤器”).在极限情况下,您可以想象偏置的Q项为0(建模偏差为随机常数),但在EKF中数值不能很好,并且由于偏差漂移而不是严格正确.

>同样,系统的初始P_0对于偏置项(数据表中记录的总可能范围)要小于完全未知的航向/角速度.

>在多轴系统中,偏置总是与轴一致(它是与其定向无关的硬件属性)但是陀螺仪对像“前进”这样的状态的影响因为带子而被旋转 – IMU.

看着EKF“学习”像陀螺仪偏差这样的值对我来说比对其他州的预测更加神奇.

标签：android,kalman-filter

来源： https://codeday.me/bug/20190517/1122943.html