卡尔曼滤波器 - 指南针和陀螺仪

时间:2013-03-07 15:45:15

标签: android kalman-filter

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

我将acc值与测量值融合以获得方向(使用旋转矩阵),并且它运行良好。

但现在我想添加陀螺仪,以便在磁传感器不准确时进行补偿。因此,我想使用kalman过滤器来融合两个结果并得到一个很好的过滤结果(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开始并且以某种速率增加而不管测量的那个(罗盘)它不会停止只是继续增加......

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

1 个答案:

答案 0 :(得分:6)

您所看到的问题是,虽然陀螺仪的噪音非常低,但并非零均值。当您使用字词e^2(gyro)时,您正在实施一个过滤器,声称z_gyro = true_gyro + v v ~ N(0, e^2) v ~ N(bias, e^2)真相更像是update(imu - bias),即使偏见有一些条款(主要是静态导通偏置加上温度漂移引起的偏移。因此,您正在整合偏差并不断旋转。

如果您校准了偏差(只是在静止时测量陀螺仪的输出),那么您可以拨打update(imu)而不是e^2(gyro)。您可能需要增加R以考虑偏差的变化,但不会像您尝试考虑所有偏差一样多(未补偿的偏移将变为与{{1成比例的固定航向位移)磁力计和陀螺仪的术语。

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

  • F中,标题与实际转弯率(dt)相关,因此状态协方差P演变了与标题和转弯率相关的非对角线条款你更新P
  • 的时间
  • 同样在H你已经描述了偏差和陀螺仪速率之间的关系,表达了“要么我转得更快,要么我有更多偏见”的想法,所以过滤器更新状态以平衡这两种可能性基于噪声协方差。
  • Q中,转弯率过程噪音必须设置得相当高,以应对您测量的任何意外运动。但偏差的Q要小得多,因为偏差不是很快发展的(实际上,最好的模型可能是一阶高斯 - 马尔可夫过程,我不会在这里解释,除了抛弃“有限内存过滤器”中另一个有用的Google术语。在极限情况下,你可以想象偏差的Q项为0(建模偏差为随机常数),但在EKF中数字效果不佳,并且不是'由于偏差漂移,严格来说是真的。
  • 同样,系统的初始P_0对于偏差项(数据表中记录的总可能范围)要小于完全未知的航向/角速度。
  • 在多轴系统中,偏置总是与轴一致(它是与其定向无关的硬件属性)但是陀螺仪对像“前进”这样的状态的影响正在被转动,因为捷联IMU。

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